diff --git a/CMakeLists.txt b/CMakeLists.txt index 35cabeb..bcdda73 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,165 +1,25 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(epuck_driver_cpp) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - tf - cv_bridge -) -find_package(OpenCV REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES epuck_driver_cpp -# CATKIN_DEPENDS roscpp std_msgs -# DEPENDS system_lib -) +## Find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() ########### ## Build ## ########### -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(epuck_driver_cpp -# src/${PROJECT_NAME}/epuck2_driver_cpp.cpp -# ) - ## Declare a cpp executable -add_executable(epuck_driver_cpp src/epuck2_driver_cpp.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(epuck_driver_cpp_node epuck_driver_cpp_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -target_link_libraries(epuck_driver_cpp bluetooth ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} -) +ament_auto_add_executable(epuck_driver_cpp src/epuck2_driver_cpp.cpp) ############# ## Install ## ############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS epuck_driver_cpp epuck_driver_cpp_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_epuck2_driver_cpp.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_auto_package( + INSTALL_TO_SHARE + config + launch + scripts + urdf +) diff --git a/config/multi_epuck2_driver_rviz.rviz b/config/multi_epuck2_driver_rviz.rviz index 8022ed7..d628f9d 100644 --- a/config/multi_epuck2_driver_rviz.rviz +++ b/config/multi_epuck2_driver_rviz.rviz @@ -1,46 +1,35 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - - /epuck2_robot_01/RobotModel1/Status1 - - /epuck2_robot_01/MagneticField1/Status1 - - /epuck2_robot_21/Odometry1/Shape1 - Splitter Ratio: 0.6912928819656372 - Tree Height: 310 - - Class: rviz/Selection + - /epuck2_robot_01/RobotModel1 + - /epuck2_robot_21/RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 739 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 + - Class: rviz_common/Tool Properties + Expanded: ~ Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: LaserScan - - Class: rviz_plugin_tutorials/Teleop - Name: Teleop - Topic: epuck_robot_1/mobile_base/cmd_vel -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 + SyncSource: "" Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 0.05000000074505806 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -56,13 +45,14 @@ Visualization Manager: Plane Cell Count: 20 Reference Frame: Value: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: false + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true - Marker Alpha: 1 - Marker Scale: 1 + Marker Scale: 0.10000000149011612 Name: TF Show Arrows: true Show Axes: true @@ -71,91 +61,131 @@ Visualization Manager: {} Update Interval: 0 Value: false - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox0 - Queue Size: 100 - Topic: /epuck_robot_0/proximity0 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/proximity0 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox1 - Queue Size: 100 - Topic: /epuck_robot_0/proximity1 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/proximity1 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox2 - Queue Size: 100 - Topic: /epuck_robot_0/proximity2 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/proximity2 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox3 - Queue Size: 100 - Topic: /epuck_robot_0/proximity3 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/proximity3 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox4 - Queue Size: 100 - Topic: /epuck_robot_0/proximity4 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/proximity4 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox5 - Queue Size: 100 - Topic: /epuck_robot_0/proximity5 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/proximity5 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox6 - Queue Size: 100 - Topic: /epuck_robot_0/proximity6 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/proximity6 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox7 - Queue Size: 100 - Topic: /epuck_robot_0/proximity7 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/proximity7 Value: true - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: true + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/robot_description Enabled: true Links: All Links Enabled: true @@ -183,14 +213,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description - TF Prefix: epuck_robot_0 + TF Prefix: epuck2_robot_0 Update Interval: 0 Value: true Visual Enabled: true - Angle Tolerance: 0.009999999776482582 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -210,7 +242,6 @@ Visualization Manager: Keep: 100 Name: Odometry Position Tolerance: 0.009999999776482582 - Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 @@ -221,60 +252,53 @@ Visualization Manager: Shaft Length: 0.05000000074505806 Shaft Radius: 0.004999999888241291 Value: Arrow - Topic: /epuck_robot_0/odom - Unreliable: false - Value: true - - Acceleration properties: - Acc. vector alpha: 1 - Acc. vector color: 255; 0; 0 - Acc. vector scale: 0.009999999776482582 - Derotate acceleration: false - Enable acceleration: true - Axes properties: - Axes scale: 0.10000000149011612 - Enable axes: false - Box properties: - Box alpha: 1 - Box color: 255; 0; 0 - Enable box: false - x_scale: 0.10000000149011612 - y_scale: 0.10000000149011612 - z_scale: 0.10000000149011612 - Class: rviz_imu_plugin/Imu - Enabled: false - Name: Imu - Queue Size: 10 - Topic: /epuck_robot_0/imu - Unreliable: false - Value: false - fixed_frame_orientation: true - - Class: rviz/Marker + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/odom + Value: true + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_0/floor Name: Floor Namespaces: {} - Queue Size: 1 + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/floor Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_0/microphone Name: Microphone Namespaces: "": true - Queue Size: 1 - Value: true - - Class: rviz/Image + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/microphone + Value: true + - Class: rviz_default_plugins/Image Enabled: false - Image Topic: /epuck_robot_0/camera Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false + Topic: + Depth: 2 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/camera Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -284,143 +308,209 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensities - Class: rviz/LaserScan + Class: rviz_default_plugins/LaserScan Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 1 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 72 + Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ - Queue Size: 10 Selectable: false Size (Pixels): 3 Size (m): 0.004999999888241291 Style: Spheres - Topic: /epuck_robot_0/scan - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/scan Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: DistanceSensor - Queue Size: 100 - Topic: /epuck_robot_0/dist_sens - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/dist_sens Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_0/mag_field_vector Name: MagneticField Namespaces: - "": true - Queue Size: 100 + {} + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/mag_field_vector Value: true - Alpha: 0.5 - Class: rviz/Map + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map Color Scheme: costmap Draw Behind: false Enabled: true Name: Map - Topic: epuck_robot_0/map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: epuck2_robot_0/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: epuck2_robot_0/map_updates Use Timestamp: false Value: true Enabled: true Name: epuck2_robot_0 - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox0 - Queue Size: 100 - Topic: /epuck_robot_1/proximity0 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/proximity0 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox1 - Queue Size: 100 - Topic: /epuck_robot_1/proximity1 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/proximity1 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox2 - Queue Size: 100 - Topic: /epuck_robot_1/proximity2 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/proximity2 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox3 - Queue Size: 100 - Topic: /epuck_robot_1/proximity3 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/proximity3 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox4 - Queue Size: 100 - Topic: /epuck_robot_1/proximity4 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/proximity4 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox5 - Queue Size: 100 - Topic: /epuck_robot_1/proximity5 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/proximity5 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox6 - Queue Size: 100 - Topic: /epuck_robot_1/proximity6 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/proximity6 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox7 - Queue Size: 100 - Topic: /epuck_robot_1/proximity7 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/proximity7 Value: true - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: true + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/robot_description Enabled: true Links: All Links Enabled: true @@ -448,14 +538,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description - TF Prefix: epuck_robot_1 + TF Prefix: epuck2_robot_1 Update Interval: 0 Value: true Visual Enabled: true - Angle Tolerance: 0.009999999776482582 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -475,7 +567,6 @@ Visualization Manager: Keep: 100 Name: Odometry Position Tolerance: 0.009999999776482582 - Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 @@ -486,60 +577,53 @@ Visualization Manager: Shaft Length: 0.05000000074505806 Shaft Radius: 0.004999999888241291 Value: Arrow - Topic: /epuck_robot_1/odom - Unreliable: false - Value: true - - Acceleration properties: - Acc. vector alpha: 1 - Acc. vector color: 255; 0; 0 - Acc. vector scale: 0.009999999776482582 - Derotate acceleration: false - Enable acceleration: true - Axes properties: - Axes scale: 0.10000000149011612 - Enable axes: false - Box properties: - Box alpha: 1 - Box color: 255; 0; 0 - Enable box: false - x_scale: 0.10000000149011612 - y_scale: 0.10000000149011612 - z_scale: 0.10000000149011612 - Class: rviz_imu_plugin/Imu - Enabled: false - Name: Imu - Queue Size: 10 - Topic: /epuck_robot_1/imu - Unreliable: false - Value: false - fixed_frame_orientation: true - - Class: rviz/Marker + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/odom + Value: true + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_1/floor Name: Floor Namespaces: {} - Queue Size: 1 + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_1/floor Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_1/microphone Name: Microphone Namespaces: "": true - Queue Size: 1 - Value: true - - Class: rviz/Image + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_1/microphone + Value: true + - Class: rviz_default_plugins/Image Enabled: false - Image Topic: /epuck_robot_1/camera Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false + Topic: + Depth: 2 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/camera Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -549,143 +633,209 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensities - Class: rviz/LaserScan + Class: rviz_default_plugins/LaserScan Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 1 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 72 + Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ - Queue Size: 10 Selectable: false Size (Pixels): 3 Size (m): 0.004999999888241291 Style: Spheres - Topic: /epuck_robot_1/scan - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/scan Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: DistanceSensor - Queue Size: 100 - Topic: /epuck_robot_1/dist_sens - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_1/dist_sens Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_1/mag_field_vector Name: MagneticField Namespaces: - "": true - Queue Size: 100 + {} + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_1/mag_field_vector Value: true - Alpha: 0.699999988079071 - Class: rviz/Map + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map - Topic: epuck_robot_1/map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: epuck2_robot_1/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: epuck2_robot_1/map_updates Use Timestamp: false Value: true Enabled: true Name: epuck2_robot_1 - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox0 - Queue Size: 100 - Topic: /epuck_robot_2/proximity0 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/proximity0 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox1 - Queue Size: 100 - Topic: /epuck_robot_2/proximity1 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/proximity1 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox2 - Queue Size: 100 - Topic: /epuck_robot_2/proximity2 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/proximity2 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox3 - Queue Size: 100 - Topic: /epuck_robot_2/proximity3 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/proximity3 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox4 - Queue Size: 100 - Topic: /epuck_robot_2/proximity4 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/proximity4 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox5 - Queue Size: 100 - Topic: /epuck_robot_2/proximity5 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/proximity5 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox6 - Queue Size: 100 - Topic: /epuck_robot_2/proximity6 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/proximity6 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox7 - Queue Size: 100 - Topic: /epuck_robot_2/proximity7 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/proximity7 Value: true - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: true + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/robot_description Enabled: true Links: All Links Enabled: true @@ -713,14 +863,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description - TF Prefix: epuck_robot_2 + TF Prefix: epuck2_robot_2 Update Interval: 0 Value: true Visual Enabled: true - Angle Tolerance: 0.009999999776482582 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -740,7 +892,6 @@ Visualization Manager: Keep: 100 Name: Odometry Position Tolerance: 0.009999999776482582 - Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 @@ -751,60 +902,53 @@ Visualization Manager: Shaft Length: 0.05000000074505806 Shaft Radius: 0.004999999888241291 Value: Arrow - Topic: /epuck_robot_2/odom - Unreliable: false - Value: true - - Acceleration properties: - Acc. vector alpha: 1 - Acc. vector color: 255; 0; 0 - Acc. vector scale: 0.009999999776482582 - Derotate acceleration: false - Enable acceleration: true - Axes properties: - Axes scale: 0.10000000149011612 - Enable axes: false - Box properties: - Box alpha: 1 - Box color: 255; 0; 0 - Enable box: false - x_scale: 0.10000000149011612 - y_scale: 0.10000000149011612 - z_scale: 0.10000000149011612 - Class: rviz_imu_plugin/Imu - Enabled: false - Name: Imu - Queue Size: 10 - Topic: /epuck_robot_2/imu - Unreliable: false - Value: false - fixed_frame_orientation: true - - Class: rviz/Marker + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/odom + Value: true + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_2/floor Name: Floor Namespaces: {} - Queue Size: 1 + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_2/floor Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_2/microphone Name: Microphone Namespaces: - {} - Queue Size: 1 - Value: true - - Class: rviz/Image + "": true + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_2/microphone + Value: true + - Class: rviz_default_plugins/Image Enabled: false - Image Topic: /epuck_robot_2/camera Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false + Topic: + Depth: 2 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/camera Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -814,143 +958,209 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensities - Class: rviz/LaserScan + Class: rviz_default_plugins/LaserScan Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 1 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 72 + Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ - Queue Size: 10 Selectable: false Size (Pixels): 3 Size (m): 0.004999999888241291 Style: Spheres - Topic: /epuck_robot_2/scan - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/scan Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: DistanceSensor - Queue Size: 100 - Topic: /epuck_robot_2/dist_sens - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_2/dist_sens Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_2/mag_field_vector Name: MagneticField Namespaces: {} - Queue Size: 100 + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_2/mag_field_vector Value: true - Alpha: 0.699999988079071 - Class: rviz/Map + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map - Topic: epuck_robot_2/map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: epuck2_robot_2/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: epuck2_robot_2/map_updates Use Timestamp: false Value: true Enabled: true Name: epuck2_robot_2 - - Class: rviz/Group + - Class: rviz_common/Group Displays: - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox0 - Queue Size: 100 - Topic: /epuck_robot_3/proximity0 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/proximity0 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox1 - Queue Size: 100 - Topic: /epuck_robot_3/proximity1 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/proximity1 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox2 - Queue Size: 100 - Topic: /epuck_robot_3/proximity2 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/proximity2 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox3 - Queue Size: 100 - Topic: /epuck_robot_3/proximity3 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/proximity3 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox4 - Queue Size: 100 - Topic: /epuck_robot_3/proximity4 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/proximity4 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox5 - Queue Size: 100 - Topic: /epuck_robot_3/proximity5 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/proximity5 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox6 - Queue Size: 100 - Topic: /epuck_robot_3/proximity6 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/proximity6 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox7 - Queue Size: 100 - Topic: /epuck_robot_3/proximity7 - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/proximity7 Value: true - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: true + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/robot_description Enabled: true Links: All Links Enabled: true @@ -978,14 +1188,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description - TF Prefix: epuck_robot_3 + TF Prefix: epuck2_robot_3 Update Interval: 0 Value: true Visual Enabled: true - Angle Tolerance: 0.009999999776482582 - Class: rviz/Odometry + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -1005,7 +1217,6 @@ Visualization Manager: Keep: 100 Name: Odometry Position Tolerance: 0.009999999776482582 - Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 @@ -1016,60 +1227,53 @@ Visualization Manager: Shaft Length: 0.05000000074505806 Shaft Radius: 0.004999999888241291 Value: Arrow - Topic: /epuck_robot_3/odom - Unreliable: false - Value: true - - Acceleration properties: - Acc. vector alpha: 1 - Acc. vector color: 255; 0; 0 - Acc. vector scale: 0.009999999776482582 - Derotate acceleration: false - Enable acceleration: true - Axes properties: - Axes scale: 0.10000000149011612 - Enable axes: false - Box properties: - Box alpha: 1 - Box color: 255; 0; 0 - Enable box: false - x_scale: 0.10000000149011612 - y_scale: 0.10000000149011612 - z_scale: 0.10000000149011612 - Class: rviz_imu_plugin/Imu - Enabled: false - Name: Imu - Queue Size: 10 - Topic: /epuck_robot_3/imu - Unreliable: false - Value: false - fixed_frame_orientation: true - - Class: rviz/Marker + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/odom + Value: true + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_3/floor Name: Floor Namespaces: {} - Queue Size: 1 + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_3/floor Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_3/microphone Name: Microphone Namespaces: - {} - Queue Size: 1 - Value: true - - Class: rviz/Image + "": true + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_3/microphone + Value: true + - Class: rviz_default_plugins/Image Enabled: false - Image Topic: /epuck_robot_3/camera Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false + Topic: + Depth: 2 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/camera Value: false - Alpha: 1 Autocompute Intensity Bounds: true @@ -1079,54 +1283,80 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensities - Class: rviz/LaserScan + Class: rviz_default_plugins/LaserScan Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 1 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 72 + Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ - Queue Size: 10 Selectable: false Size (Pixels): 3 Size (m): 0.004999999888241291 Style: Spheres - Topic: /epuck_robot_3/scan - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/scan Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: DistanceSensor - Queue Size: 100 - Topic: /epuck_robot_3/dist_sens - Unreliable: false + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_3/dist_sens Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /epuck_robot_3/mag_field_vector Name: MagneticField Namespaces: {} - Queue Size: 100 + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_3/mag_field_vector Value: true - Alpha: 0.699999988079071 - Class: rviz/Map + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map - Topic: epuck_robot_3/map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: epuck2_robot_3/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: epuck2_robot_3/map_updates Use Timestamp: false Value: true Enabled: true @@ -1134,70 +1364,86 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: map + Fixed Frame: earth Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853892654180527 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/XYOrbit - Distance: 0.5882759094238281 + Class: rviz_default_plugins/XYOrbit + Distance: 0.9149538278579712 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 Focal Point: - X: -0.002875499427318573 - Y: 0.05071228742599487 - Z: 8.456038358417572e-08 - Focal Shape Fixed Size: true + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7147977352142334 - Target Frame: base_link - Yaw: 1.7404003143310547 + Pitch: 0.3903975188732147 + Target Frame: earth + Value: XYOrbit (rviz_default_plugins) + Yaw: 2.3554189205169678 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 752 + Height: 1043 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000017d00000252fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001c1000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f007001000002040000008b0000004700fffffffb0000000a0049006d00610067006500000002d1000000c60000001600fffffffb0000000a0049006d00610067006500000002d0000000c70000001600fffffffb0000000a0049006d00610067006500000002d1000000c60000001600ffffff000000010000012c00000252fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000252000000a400fffffffb0000000a0049006d0061006700650000000274000001230000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056a0000003efc0100000002fb0000000800540069006d006501000000000000056a000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002b50000025200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000017500000371fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000371000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f007001000002040000008b0000000000000000fb0000000a0049006d00610067006500000002d1000000c60000002900fffffffb0000000a0049006d00610067006500000002d0000000c70000002900fffffffb0000000a0049006d00610067006500000002d1000000c60000002900ffffff000000010000012c00000371fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f00000371000000a900fffffffb0000000a0049006d0061006700650000000274000001230000002900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000004d30000037100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Teleop: - collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1386 - X: 341 - Y: 39 + Width: 1920 + X: 0 + Y: 204 diff --git a/config/single_epuck2_driver_rviz.rviz b/config/single_epuck2_driver_rviz.rviz index 0fd29c8..532f618 100644 --- a/config/single_epuck2_driver_rviz.rviz +++ b/config/single_epuck2_driver_rviz.rviz @@ -1,45 +1,37 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: - Expanded: - - /Odometry1/Shape1 - - /Odometry1/Covariance1 - Splitter Ratio: 0.639118016 - Tree Height: 336 - - Class: rviz/Selection + Expanded: ~ + Splitter Ratio: 0.6391180157661438 + Tree Height: 468 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 + - Class: rviz_common/Tool Properties + Expanded: ~ Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: LaserScan - - Class: rviz_plugin_tutorials/Teleop - Name: Teleop - Topic: mobile_base/cmd_vel Visualization Manager: Class: "" Displays: - Alpha: 0.5 - Cell Size: 0.0500000007 - Class: rviz/Grid + Cell Size: 0.05000000074505806 + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -53,86 +45,120 @@ Visualization Manager: Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox0 - Queue Size: 100 - Topic: /proximity0 - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/proximity0 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox1 - Queue Size: 100 - Topic: /proximity1 - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/proximity1 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox2 - Queue Size: 100 - Topic: /proximity2 - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/proximity2 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox3 - Queue Size: 100 - Topic: /proximity3 - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/proximity3 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox4 - Queue Size: 100 - Topic: /proximity4 - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/proximity4 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox5 - Queue Size: 100 - Topic: /proximity5 - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/proximity5 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox6 - Queue Size: 100 - Topic: /proximity6 - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/proximity6 Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: Prox7 - Queue Size: 100 - Topic: /proximity7 - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/proximity7 Value: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: false + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true @@ -146,8 +172,16 @@ Visualization Manager: Update Interval: 0 Value: false - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/robot_description Enabled: true Links: All Links Enabled: true @@ -175,14 +209,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: epuck2_robot_0 Update Interval: 0 Value: true Visual Enabled: true - - Angle Tolerance: 0.00999999978 - Class: rviz/Odometry + - Angle Tolerance: 0.009999999776482582 + Class: rviz_default_plugins/Odometry Covariance: Orientation: Alpha: 0.5 @@ -193,7 +229,7 @@ Visualization Manager: Scale: 1 Value: true Position: - Alpha: 0.300000012 + Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true @@ -201,70 +237,64 @@ Visualization Manager: Enabled: true Keep: 100 Name: Odometry - Position Tolerance: 0.00999999978 + Position Tolerance: 0.009999999776482582 Shape: Alpha: 1 Axes Length: 1 - Axes Radius: 0.100000001 + Axes Radius: 0.10000000149011612 Color: 255; 25; 0 - Head Length: 0.00999999978 - Head Radius: 0.00999999978 - Shaft Length: 0.0500000007 - Shaft Radius: 0.00499999989 + Head Length: 0.009999999776482582 + Head Radius: 0.009999999776482582 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.004999999888241291 Value: Arrow - Topic: /odom - Unreliable: false - Value: true - - Acceleration properties: - Acc. vector alpha: 1 - Acc. vector color: 255; 0; 0 - Acc. vector scale: 0.00999999978 - Derotate acceleration: false - Enable acceleration: true - Axes properties: - Axes scale: 0.100000001 - Enable axes: false - Box properties: - Box alpha: 1 - Box color: 255; 0; 0 - Enable box: false - x_scale: 0.100000001 - y_scale: 0.100000001 - z_scale: 0.100000001 - Class: rviz_imu_plugin/Imu - Enabled: true - Name: Imu - Topic: /imu - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/odom Value: true - fixed_frame_orientation: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /floor Name: Floor Namespaces: {} - Queue Size: 1 + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/floor Value: true - - Class: rviz/Marker + - Class: rviz_default_plugins/Marker Enabled: true - Marker Topic: /microphone Name: Microphone Namespaces: "": true - Queue Size: 1 + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/microphone Value: true - - Class: rviz/Image + - Class: rviz_default_plugins/Image Enabled: true - Image Topic: /camera Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false + Topic: + Depth: 2 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/camera Value: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -274,100 +304,147 @@ Visualization Manager: Value: true Axis: Z Channel Name: intensities - Class: rviz/LaserScan + Class: rviz_default_plugins/LaserScan Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 1 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 72 + Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan Position Transformer: XYZ - Queue Size: 10 Selectable: false Size (Pixels): 3 - Size (m): 0.00499999989 + Size (m): 0.004999999888241291 Style: Spheres - Topic: /scan - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/scan Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 0.5 - Class: rviz/Map + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map Color Scheme: costmap Draw Behind: false Enabled: true Name: Map - Topic: /map - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /epuck2_robot_0/map_updates Use Timestamp: false Value: true - Alpha: 0.5 Buffer Length: 1 - Class: rviz/Range + Class: rviz_default_plugins/Range Color: 255; 255; 255 Enabled: true Name: DistanceSensor - Queue Size: 100 - Topic: /dist_sens - Unreliable: false + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 1 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/dist_sens Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /mag_field_vector + - Class: rviz_default_plugins/Marker + Enabled: false Name: MagneticField Namespaces: - "": true - Queue Size: 100 - Value: true + {} + Topic: + Depth: 100 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /epuck2_robot_0/mag_field_vector + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: map + Fixed Frame: earth Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/XYOrbit - Distance: 0.865584314 + Class: rviz_default_plugins/XYOrbit + Distance: 0.8561939597129822 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.00287549943 - Y: 0.0507122874 - Z: 8.45603836e-08 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.714797795 - Target Frame: base_link - Value: XYOrbit (rviz) - Yaw: 1.7203995 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3403986096382141 + Target Frame: earth + Value: XYOrbit (rviz_default_plugins) + Yaw: 0.8103981614112854 Saved: ~ Window Geometry: Displays: @@ -377,11 +454,9 @@ Window Geometry: Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000017d0000027afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001df000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f0070010000020d000000950000004900ffffff000000010000012c0000027afc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000001a4000000ad00fffffffb0000000a0049006d00610067006501000001d2000000d00000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000034a0000027a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000017900000262fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000262000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00540065006c0065006f0070010000020d000000950000000000000000000000010000012c00000262fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f00000194000000a900fffffffb0000000a0049006d00610067006501000001d9000000c80000002900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff0000026f00fffffffb0000000800540069006d006501000000000000045000000000000000000000034e0000026200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Teleop: - collapsed: false Time: collapsed: false Tool Properties: @@ -390,4 +465,4 @@ Window Geometry: collapsed: false Width: 1535 X: 226 - Y: 61 + Y: 204 diff --git a/launch/epuck2_controller.launch b/launch/epuck2_controller.launch deleted file mode 100644 index 3bf1a5c..0000000 --- a/launch/epuck2_controller.launch +++ /dev/null @@ -1,122 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/epuck2_controller.launch.py b/launch/epuck2_controller.launch.py new file mode 100644 index 0000000..b761827 --- /dev/null +++ b/launch/epuck2_controller.launch.py @@ -0,0 +1,221 @@ +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import PathJoinSubstitution, FileContent +import launch +from launch.conditions import IfCondition +import launch_ros.actions + + +def generate_launch_description(): + launch_args = list( + map( + lambda x: launch.actions.DeclareLaunchArgument( + name=x[0], + default_value=x[1], + ), + { + "namespace": "/", + "epuck2_id": "0", + "epuck2_address": "192.168.1.1", + "epuck2_port": "1000", + "epuck2_name": "epuck2_robot_0", + "rviz": "true", + "xpos": "0.0", + "ypos": "0.0", + "theta": "0.0", + "cam_en": "false", + "floor_en": "false", + "sim_en": "false", + }.items(), + ) + ) + + ld = launch.LaunchDescription( + launch_args + + [ + launch_ros.actions.Node( + condition=IfCondition( + launch.substitutions.LaunchConfiguration("rviz") + ), + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=[ + "-d", + PathJoinSubstitution( + [ + FindPackageShare("epuck_driver_cpp"), + "config", + "single_epuck2_driver_rviz.rviz", + ] + ), + ], + ), + # Optionally launch a simple server to simulate the connection to the e-puck2 robot + launch.actions.execute_process.ExecuteProcess( + name="epuck2_sim", + condition=IfCondition( + launch.substitutions.LaunchConfiguration("sim_en") + ), + output="screen", + cmd=[ + "python3", + [ + FindPackageShare("epuck_driver_cpp"), + "/scripts/epuck2_sim", + ], + "--ip", + launch.substitutions.LaunchConfiguration("epuck2_address"), + "--port", + launch.substitutions.LaunchConfiguration("epuck2_port"), + ], + ), + launch_ros.actions.Node( + package="epuck_driver_cpp", + executable="epuck_driver_cpp", + name="epuck_driver_cpp", + namespace=launch.substitutions.LaunchConfiguration("namespace"), + output="screen", + prefix=[ + # Debugging with gdb + # "xterm -bg black -fg white -fa 'Monospace' -fs 13 -e gdb -ex start --args" + ], + parameters=[ + { + "epuck2_id": launch.substitutions.LaunchConfiguration( + "epuck2_id" + ) + }, + { + "epuck2_address": launch.substitutions.LaunchConfiguration( + "epuck2_address" + ) + }, + { + "epuck2_port": launch.substitutions.LaunchConfiguration( + "epuck2_port" + ) + }, + { + "epuck2_name": launch.substitutions.LaunchConfiguration( + "epuck2_name" + ) + }, + {"robot/xpos": launch.substitutions.LaunchConfiguration("xpos")}, + {"robot/ypos": launch.substitutions.LaunchConfiguration("ypos")}, + {"robot/theta": launch.substitutions.LaunchConfiguration("theta")}, + { + "robot/camera": launch.substitutions.LaunchConfiguration( + "cam_en" + ) + }, + { + "robot/floor": launch.substitutions.LaunchConfiguration( + "floor_en" + ) + }, + ], + ), + launch_ros.actions.Node( + namespace=launch.substitutions.LaunchConfiguration("namespace"), + package="robot_state_publisher", + executable="robot_state_publisher", + name=( + "epuck_state_publisher_", + launch.substitutions.LaunchConfiguration("epuck2_id"), + ), + parameters=[ + {"use_sim_time": True}, + { + "frame_prefix": [ + launch.substitutions.LaunchConfiguration("epuck2_name"), + "/", + ], + "publish_frequency": 60.0, + "robot_description": FileContent( + [ + PathJoinSubstitution( + [ + FindPackageShare("epuck_driver_cpp"), + "urdf", + "epuck_urdf.xml", + ] + ), + ] + ), + }, + ], + ), + launch_ros.actions.Node( + namespace=launch.substitutions.LaunchConfiguration("namespace"), + package="joint_state_publisher", + executable="joint_state_publisher", + name=( + "epuck_joint_publisher_", + launch.substitutions.LaunchConfiguration("epuck2_id"), + ), + arguments=[ + PathJoinSubstitution( + [ + FindPackageShare("epuck_driver_cpp"), + "urdf", + "epuck_urdf.xml", + ] + ), + ], + ), + # launch_ros.actions.Node( + # package="gmapping", + # executable="slam_gmapping", + # name="slam_gmapping", + # output="screen", + # parameters=[ + # { + # "base_frame": [ + # launch.substitutions.LaunchConfiguration("epuck2_name"), + # launch.substitutions.TextSubstitution( + # text=' + "/base_link"' + # ), + # ], + # }, + # {"map_update_interval": "1.0"}, + # {"linearUpdate": "0.01"}, + # {"angularUpdate": "0.09"}, + # {"temporalUpdate": "-1.0"}, + # {"xmin": "-1.0"}, + # {"xmax": "1.0"}, + # {"ymin": "-1.0"}, + # {"ymax": "1.0"}, + # {"delta": "0.005"}, + # {"maxRange": "0.080"}, + # {"maxUrange": "0.075"}, + # {"sigma": "0.05"}, + # {"kernelSize": "1"}, + # {"lstep": "0.01"}, + # {"astep": "0.02"}, + # {"iterations": "5"}, + # {"lsigma": "0.075"}, + # {"ogain": "3.0"}, + # {"lskip": "0"}, + # {"minimumScore": "100.0"}, + # {"srr": "0.01"}, + # {"srt": "0.02"}, + # {"str": "0.01"}, + # {"stt": "0.02"}, + # {"resampleThreshold": "0.5"}, + # {"particles": "30"}, + # {"llsamplerange": "0.01"}, + # {"llsamplestep": "0.01"}, + # {"lasamplerange": "0.005"}, + # {"lasamplestep": "0.005"}, + # {"occ_thresh": "0.25"}, + # {"transform_publish_period": "0.05"}, + # ], + # ), + ] + ) + return ld + + +if __name__ == "__main__": + generate_launch_description() diff --git a/launch/multi_epuck2.launch b/launch/multi_epuck2.launch deleted file mode 100644 index 2ce6bd4..0000000 --- a/launch/multi_epuck2.launch +++ /dev/null @@ -1,76 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/multi_epuck2.launch.py b/launch/multi_epuck2.launch.py new file mode 100644 index 0000000..350ee88 --- /dev/null +++ b/launch/multi_epuck2.launch.py @@ -0,0 +1,124 @@ +import launch +import launch_ros.actions +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import ( + PathJoinSubstitution, + LaunchConfiguration, + PythonExpression, +) + + +def generate_launch_description(): + launch_args = list( + map( + lambda x: launch.actions.DeclareLaunchArgument( + name=x[0], + default_value=x[1], + ), + { + "robot_id0": "3000", + "robot_id1": "0", # 0 means not used + "robot_id2": "0", # 0 means not used + "robot_id3": "0", # 0 means not used + "robot_addr0": "192.168.1.1", + "robot_addr1": "192.168.1.2", + "robot_addr2": "192.168.1.3", + "robot_addr3": "192.168.1.4", + "robot_port0": "1000", + "robot_port1": "1000", + "robot_port2": "1000", + "robot_port3": "1000", + "robot_sim_en0": "false", + "robot_sim_en1": "false", + "robot_sim_en2": "false", + "robot_sim_en3": "false", + "rviz": "true", + }.items(), + ) + ) + + robot_poses = [ + ("-0.1", "-0.1", "0.0"), + ("0.1", "-0.1", "0.0"), + ("-0.1", "0.1", "0.0"), + ("0.1", "0.1", "0.0"), + ] + + launch_robots = [] + for i in range(4): + launch_robots.append( + launch.actions.IncludeLaunchDescription( + launch.launch_description_sources.PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("epuck_driver_cpp"), + "launch", + "epuck2_controller.launch.py", + ] + ) + ), + condition=launch.conditions.IfCondition( + PythonExpression( + [ + f"{i} == 0 or (", + launch.substitutions.LaunchConfiguration( + f"robot_id{i}"), + " != 0)", + ] + ) + ), + launch_arguments={ + "namespace": f"epuck2_robot_{i}", + "epuck2_id": launch.substitutions.LaunchConfiguration( + f"robot_id{i}" + ), + "epuck2_address": launch.substitutions.LaunchConfiguration( + f"robot_addr{i}" + ), + "epuck2_port": launch.substitutions.LaunchConfiguration( + f"robot_port{i}" + ), + "epuck2_name": f"epuck2_robot_{i}", + "cam_en": "false", + "floor_en": "false", + "xpos": robot_poses[i][0], + "ypos": robot_poses[i][1], + "theta": robot_poses[i][2], + "rviz": "false", + "sim_en": launch.substitutions.LaunchConfiguration( + f"robot_sim_en{i}" + ), + }.items(), + ) + ) + + ld = launch.LaunchDescription( + launch_args + + launch_robots + + [ + launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + condition=launch.conditions.IfCondition( + launch.substitutions.LaunchConfiguration("rviz") + ), + arguments=[ + "-d", + PathJoinSubstitution( + [ + FindPackageShare("epuck_driver_cpp"), + "config", + "multi_epuck2_driver_rviz.rviz", + ] + ), + ], + ), + ] + ) + return ld + + +if __name__ == "__main__": + generate_launch_description() diff --git a/package.xml b/package.xml index c6cea51..11852f2 100644 --- a/package.xml +++ b/package.xml @@ -1,10 +1,11 @@ - + + epuck_driver_cpp 0.0.0 The epuck_driver_cpp package - + stefano.gctronic @@ -15,43 +16,21 @@ GPLv + ament_cmake_auto - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - tf - cv_bridge - roscpp - std_msgs - tf - cv_bridge + rclcpp + std_msgs + geometry_msgs + nav_msgs + visualization_msgs + tf2 + tf2_ros + tf2_geometry_msgs + cv_bridge + image_transport - - + ament_cmake diff --git a/scripts/epuck2_recv b/scripts/epuck2_recv new file mode 100755 index 0000000..da8bba5 --- /dev/null +++ b/scripts/epuck2_recv @@ -0,0 +1,287 @@ +#!/usr/bin/env python3 + +# Run a simple socket server to test the e-puck driver +# Usage: epuck_sim [-h] [--ip IP] [--port PORT] + +import argparse +import socket +import struct +import time +from dataclasses import dataclass + +ENDIAN_FMT = "<" + +EPUCK_SENSOR_PACKET_FMT_STR: str = ( + ENDIAN_FMT + "BhhhfffhhhfffBHHHHHHHHHHHHHHHHHHHHHhhHBBBBBHHHHHHBB" +) +EPUCK_SENSOR_PACKET_ID: int = 0x2 + + +@dataclass +class EpuckSensorPacket: + id: int = 0x2 # byte + imu_acc_x: int = 0 # short + imu_acc_y: int = 0 # short + imu_acc_z: int = 0 # short + imu_acceleration: float = 0 + imu_orientation: float = 0 + imu_inclination: float = 0 + imu_gyro_x: int = 0 # short + imu_gyro_y: int = 0 # short + imu_gyro_z: int = 0 # short + imu_magnetometer_x: float = 0 + imu_magnetometer_y: float = 0 + imu_magnetometer_z: float = 0 + imu_temp: int = 0 # byte + ir_proximity_0: int = 0 # unsigned short + ir_proximity_1: int = 0 # unsigned short + ir_proximity_2: int = 0 # unsigned short + ir_proximity_3: int = 0 # unsigned short + ir_proximity_4: int = 0 # unsigned short + ir_proximity_5: int = 0 # unsigned short + ir_proximity_6: int = 0 # unsigned short + ir_proximity_7: int = 0 # unsigned short + ir_ambient_0: int = 0 # unsigned short + ir_ambient_1: int = 0 # unsigned short + ir_ambient_2: int = 0 # unsigned short + ir_ambient_3: int = 0 # unsigned short + ir_ambient_4: int = 0 # unsigned short + ir_ambient_5: int = 0 # unsigned short + ir_ambient_6: int = 0 # unsigned short + ir_ambient_7: int = 0 # unsigned short + tof_distance: int = 0 # unsigned short + mic_0: int = 0 # unsigned short + mic_1: int = 0 # unsigned short + mic_2: int = 0 # unsigned short + mic_3: int = 0 # unsigned short + motor_left_steps: int = 0 # short + motor_right_steps: int = 0 # short + bat_adc_value: int = 0 # unsigned short + usd_state: int = 0 # byte + tv_remote_toggle: int = 0 # byte + tv_remote_addr: int = 0 # byte + tv_remote_data: int = 0 # byte + selector_position: int = 0 # byte + ground_sensor_proximity_0: int = 0 # unsigned short + ground_sensor_proximity_1: int = 0 # unsigned short + ground_sensor_proximity_2: int = 0 # unsigned short + ground_sensor_ambient_0: int = 0 # unsigned short + ground_sensor_ambient_1: int = 0 # unsigned short + ground_sensor_ambient_2: int = 0 # unsigned short + button_state: int = 0 # byte + reserved_empty: int = 0 # byte + + def pack(self): + return struct.pack( + EPUCK_SENSOR_PACKET_FMT_STR, + self.id, + self.imu_acc_x, + self.imu_acc_y, + self.imu_acc_z, + self.imu_acceleration, + self.imu_orientation, + self.imu_inclination, + self.imu_gyro_x, + self.imu_gyro_y, + self.imu_gyro_z, + self.imu_magnetometer_x, + self.imu_magnetometer_y, + self.imu_magnetometer_z, + self.imu_temp, + self.ir_proximity_0, + self.ir_proximity_1, + self.ir_proximity_2, + self.ir_proximity_3, + self.ir_proximity_4, + self.ir_proximity_5, + self.ir_proximity_6, + self.ir_proximity_7, + self.ir_ambient_0, + self.ir_ambient_1, + self.ir_ambient_2, + self.ir_ambient_3, + self.ir_ambient_4, + self.ir_ambient_5, + self.ir_ambient_6, + self.ir_ambient_7, + self.tof_distance, + self.mic_0, + self.mic_1, + self.mic_2, + self.mic_3, + self.motor_left_steps, + self.motor_right_steps, + self.bat_adc_value, + self.usd_state, + self.tv_remote_toggle, + self.tv_remote_addr, + self.tv_remote_data, + self.selector_position, + self.ground_sensor_proximity_0, + self.ground_sensor_proximity_1, + self.ground_sensor_proximity_2, + self.ground_sensor_ambient_0, + self.ground_sensor_ambient_1, + self.ground_sensor_ambient_2, + self.button_state, + self.reserved_empty, + ) + + @classmethod + def unpack(cls, buffer: bytes): + if buffer[0] != EPUCK_SENSOR_PACKET_ID: + raise ValueError( + f"Invalid message id: {buffer[0]}, expected {EPUCK_SENSOR_PACKET_ID}" + ) + args = struct.unpack(EPUCK_SENSOR_PACKET_FMT_STR, buffer) + return cls(*args) + + @classmethod + def calcsize(cls): + return struct.calcsize(EPUCK_SENSOR_PACKET_FMT_STR) + + +EPUCK_COMMAND_PACKET_FMT_STR: str = ENDIAN_FMT + "BBBhhBBBBBBBBBBBBBB" +EPUCK_COMMAND_PACKET_ID: int = 0x80 + + +@dataclass +class EpuckCommandPacket: + command: int = 0x80 # byte + request_flags: int = 0 # byte + settings_flags: int = 0 # byte + motors_left: int = 0 # short + motors_right: int = 0 # short + leds_state: int = 0 # byte + rgb_leds_led2_r: int = 0 # byte + rgb_leds_led2_g: int = 0 # byte + rgb_leds_led2_b: int = 0 # byte + rgb_leds_led4_r: int = 0 # byte + rgb_leds_led4_g: int = 0 # byte + rgb_leds_led4_b: int = 0 # byte + rgb_leds_led6_r: int = 0 # byte + rgb_leds_led6_g: int = 0 # byte + rgb_leds_led6_b: int = 0 # byte + rgb_leds_led8_r: int = 0 # byte + rgb_leds_led8_g: int = 0 # byte + rgb_leds_led8_b: int = 0 # byte + speaker_sound_id: int = 0 # byte + + def pack(self): + return struct.pack( + EpuckCommandPacket.FMT_STR, + self.command, + self.request_flags, + self.settings_flags, + self.motors_left, + self.motors_right, + self.leds_state, + self.rgb_leds_led2_r, + self.rgb_leds_led2_g, + self.rgb_leds_led2_b, + self.rgb_leds_led4_r, + self.rgb_leds_led4_g, + self.rgb_leds_led4_b, + self.rgb_leds_led6_r, + self.rgb_leds_led6_g, + self.rgb_leds_led6_b, + self.rgb_leds_led8_r, + self.rgb_leds_led8_g, + self.rgb_leds_led8_b, + self.speaker_sound_id, + ) + + @classmethod + def unpack(cls, buffer: bytes): + if buffer[0] != EPUCK_COMMAND_PACKET_ID: + raise ValueError( + f"Invalid message id: {buffer[0]}, expected {EPUCK_COMMAND_PACKET_ID}" + ) + args = struct.unpack(EPUCK_COMMAND_PACKET_FMT_STR, buffer) + # 0001 1001 + return cls(*args) + + @classmethod + def calcsize(cls): + return struct.calcsize(EPUCK_COMMAND_PACKET_FMT_STR) + + +sock: socket.socket = None +IMG_SIZE = 160 * 120 * 2 +SENSOR_SIZE = EpuckSensorPacket.calcsize() + + +def initConnectionWithRobot(server_address): + # Create a TCP/IP socket + global sock + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + + # Bind the socket to the address and port + print("Starting up on {} port {}".format(*server_address)) + sock.connect(("192.168.0.22", 1000)) + + +def closeConnection(): + sock.close() + + +def updateSensorsAndActuators(): + camera_enabled = False + + bytes_sent = 0 + image = bytes([]) + sensor = bytes([]) + + bytes_sent = 0 + command = EpuckCommandPacket(request_flags=0b11 if camera_enabled else 0b10).pack() + while bytes_sent < EpuckCommandPacket.calcsize(): + bytes_sent += sock.send(command[bytes_sent:], 0) + + if camera_enabled: + expected_recv_packets = 2 + else: + expected_recv_packets = 1 + + while expected_recv_packets > 0: + header = sock.recv(1, 0) + + if header[0] == 0x01: # Camera. + image = header + sock.recv(IMG_SIZE - 1, 0) + + elif header[0] == 0x02: # Sensors. + sensor = header + sock.recv(SENSOR_SIZE - 1, 0) + + elif header[0] == 0x03: + print("empty packet") + + else: + print("unexpected packet") + + expected_recv_packets -= 1 + + return image, sensor + + +def main(server_address): + # Create a TCP/IP socket + initConnectionWithRobot(server_address) + + while True: + _, sensor = updateSensorsAndActuators() + + epuck_sensors = EpuckSensorPacket.unpack(sensor) + print(epuck_sensors.imu_acceleration) + + time.sleep(0.01) + + +if __name__ == "__main__": + arg_parser = argparse.ArgumentParser(description="Test server for e-puck driver") + arg_parser.add_argument( + "--ip", type=str, default="192.168.0.2", help="IP address of this server" + ) + arg_parser.add_argument( + "--port", type=int, default=10000, help="Port number of this server" + ) + args = arg_parser.parse_args() + main((args.ip, args.port)) diff --git a/scripts/epuck2_sim b/scripts/epuck2_sim new file mode 100755 index 0000000..8cc7692 --- /dev/null +++ b/scripts/epuck2_sim @@ -0,0 +1,327 @@ +#!/usr/bin/env python3 + +# Run a simple socket server to test the e-puck driver +# Usage: epuck_sim [-h] [--ip IP] [--port PORT] + +import argparse +import signal +import socket +import struct +import time +from dataclasses import dataclass + + +ENDIAN_FMT = "<" + +EPUCK_SENSOR_PACKET_FMT_STR: str = ( + ENDIAN_FMT + "BhhhfffhhhfffBHHHHHHHHHHHHHHHHHHHHHhhHBBBBBHHHHHHBB" +) +EPUCK_SENSOR_PACKET_ID: int = 0x2 + + +@dataclass +class EpuckSensorPacket: + id: int = 0x2 # byte + imu_acc_x: int = 0 # short + imu_acc_y: int = 0 # short + imu_acc_z: int = 0 # short + imu_acceleration: float = 0 + imu_orientation: float = 0 + imu_inclination: float = 0 + imu_gyro_x: int = 0 # short + imu_gyro_y: int = 0 # short + imu_gyro_z: int = 0 # short + imu_magnetometer_x: float = 0 + imu_magnetometer_y: float = 0 + imu_magnetometer_z: float = 0 + imu_temp: int = 0 # byte + ir_proximity_0: int = 0 # unsigned short + ir_proximity_1: int = 0 # unsigned short + ir_proximity_2: int = 0 # unsigned short + ir_proximity_3: int = 0 # unsigned short + ir_proximity_4: int = 0 # unsigned short + ir_proximity_5: int = 0 # unsigned short + ir_proximity_6: int = 0 # unsigned short + ir_proximity_7: int = 0 # unsigned short + ir_ambient_0: int = 0 # unsigned short + ir_ambient_1: int = 0 # unsigned short + ir_ambient_2: int = 0 # unsigned short + ir_ambient_3: int = 0 # unsigned short + ir_ambient_4: int = 0 # unsigned short + ir_ambient_5: int = 0 # unsigned short + ir_ambient_6: int = 0 # unsigned short + ir_ambient_7: int = 0 # unsigned short + tof_distance: int = 0 # unsigned short + mic_0: int = 0 # unsigned short + mic_1: int = 0 # unsigned short + mic_2: int = 0 # unsigned short + mic_3: int = 0 # unsigned short + motor_left_steps: int = 0 # short + motor_right_steps: int = 0 # short + bat_adc_value: int = 0 # unsigned short + usd_state: int = 0 # byte + tv_remote_toggle: int = 0 # byte + tv_remote_addr: int = 0 # byte + tv_remote_data: int = 0 # byte + selector_position: int = 0 # byte + ground_sensor_proximity_0: int = 0 # unsigned short + ground_sensor_proximity_1: int = 0 # unsigned short + ground_sensor_proximity_2: int = 0 # unsigned short + ground_sensor_ambient_0: int = 0 # unsigned short + ground_sensor_ambient_1: int = 0 # unsigned short + ground_sensor_ambient_2: int = 0 # unsigned short + button_state: int = 0 # byte + reserved_empty: int = 0 # byte + + def pack(self): + return struct.pack( + EPUCK_SENSOR_PACKET_FMT_STR, + self.id, + self.imu_acc_x, + self.imu_acc_y, + self.imu_acc_z, + self.imu_acceleration, + self.imu_orientation, + self.imu_inclination, + self.imu_gyro_x, + self.imu_gyro_y, + self.imu_gyro_z, + self.imu_magnetometer_x, + self.imu_magnetometer_y, + self.imu_magnetometer_z, + self.imu_temp, + self.ir_proximity_0, + self.ir_proximity_1, + self.ir_proximity_2, + self.ir_proximity_3, + self.ir_proximity_4, + self.ir_proximity_5, + self.ir_proximity_6, + self.ir_proximity_7, + self.ir_ambient_0, + self.ir_ambient_1, + self.ir_ambient_2, + self.ir_ambient_3, + self.ir_ambient_4, + self.ir_ambient_5, + self.ir_ambient_6, + self.ir_ambient_7, + self.tof_distance, + self.mic_0, + self.mic_1, + self.mic_2, + self.mic_3, + self.motor_left_steps, + self.motor_right_steps, + self.bat_adc_value, + self.usd_state, + self.tv_remote_toggle, + self.tv_remote_addr, + self.tv_remote_data, + self.selector_position, + self.ground_sensor_proximity_0, + self.ground_sensor_proximity_1, + self.ground_sensor_proximity_2, + self.ground_sensor_ambient_0, + self.ground_sensor_ambient_1, + self.ground_sensor_ambient_2, + self.button_state, + self.reserved_empty, + ) + + @classmethod + def unpack(cls, buffer: bytes): + if buffer[0] != EPUCK_SENSOR_PACKET_ID: + raise ValueError( + f"Invalid message id: {buffer[0]}, expected {EPUCK_SENSOR_PACKET_ID}" + ) + args = struct.unpack(EPUCK_SENSOR_PACKET_FMT_STR, buffer) + return cls(*args) + + @classmethod + def calcsize(cls): + return struct.calcsize(EPUCK_SENSOR_PACKET_FMT_STR) + + +EPUCK_COMMAND_PACKET_FMT_STR: str = ENDIAN_FMT + "BBBhhBBBBBBBBBBBBBB" +EPUCK_COMMAND_PACKET_ID: int = 0x80 + + +@dataclass +class EpuckCommandPacket: + command: int = 0x80 # byte + request_flags: int = 0 # byte + settings_flags: int = 0 # byte + motors_left: int = 0 # short + motors_right: int = 0 # short + leds_state: int = 0 # byte + rgb_leds_led2_r: int = 0 # byte + rgb_leds_led2_g: int = 0 # byte + rgb_leds_led2_b: int = 0 # byte + rgb_leds_led4_r: int = 0 # byte + rgb_leds_led4_g: int = 0 # byte + rgb_leds_led4_b: int = 0 # byte + rgb_leds_led6_r: int = 0 # byte + rgb_leds_led6_g: int = 0 # byte + rgb_leds_led6_b: int = 0 # byte + rgb_leds_led8_r: int = 0 # byte + rgb_leds_led8_g: int = 0 # byte + rgb_leds_led8_b: int = 0 # byte + speaker_sound_id: int = 0 # byte + + def pack(self): + return struct.pack( + EpuckCommandPacket.FMT_STR, + self.command, + self.request_flags, + self.settings_flags, + self.motors_left, + self.motors_right, + self.leds_state, + self.rgb_leds_led2_r, + self.rgb_leds_led2_g, + self.rgb_leds_led2_b, + self.rgb_leds_led4_r, + self.rgb_leds_led4_g, + self.rgb_leds_led4_b, + self.rgb_leds_led6_r, + self.rgb_leds_led6_g, + self.rgb_leds_led6_b, + self.rgb_leds_led8_r, + self.rgb_leds_led8_g, + self.rgb_leds_led8_b, + self.speaker_sound_id, + ) + + @classmethod + def unpack(cls, buffer: bytes): + if buffer[0] != EPUCK_COMMAND_PACKET_ID: + raise ValueError( + f"Invalid message id: {buffer[0]}, expected {EPUCK_COMMAND_PACKET_ID}" + ) + args = struct.unpack(EPUCK_COMMAND_PACKET_FMT_STR, buffer) + # 0001 1001 + return cls(*args) + + @classmethod + def calcsize(cls): + return struct.calcsize(EPUCK_COMMAND_PACKET_FMT_STR) + + +class EpuckModel: + wl_pos: int = 0 + wr_pos: int = 0 + wl_vel: float = 0 + wr_vel: float = 0 + + def get_steps(self, dt): + wl_steps = self.wl_pos + round(self.wl_vel * dt) + wr_steps = self.wr_pos + round(self.wr_vel * dt) + + # Steps are between -32768-32767 + if wl_steps > 32767: + wl_steps -= 65536 + elif wl_steps < -32768: + wl_steps += 65536 + + if wr_steps > 32767: + wr_steps -= 65536 + elif wr_steps < -32768: + wr_steps += 65536 + + self.wl_pos = wl_steps + self.wr_pos = wr_steps + + return self.wl_pos, self.wr_pos + + def update_speed(self, msg: EpuckCommandPacket): + self.wl_vel = msg.motors_left # steps / second + self.wr_vel = msg.motors_right + return + + +sock: socket.socket + + +def main(server_address): + # Create a TCP/IP socket + global sock + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + + # Bind the socket to the address and port + print("Starting up on {} port {}".format(*server_address)) + sock.bind(server_address) + + # Listen for incoming connections + sock.listen(1) + + model = EpuckModel() + + while True: + # Wait for a connection + print("Waiting for a connection") + connection, client_address = sock.accept() + try: + print("Connection from", client_address) + last_update_time = time.time() + + # Receive the command data and respond appropriately + while True: + data = connection.recv(EpuckCommandPacket.calcsize()) + if data: + # Unpack the data into a command message + msg = EpuckCommandPacket.unpack(data) + start_img_stream = (msg.request_flags & 0b01) > 0 + start_sensors_stream = (msg.request_flags & 0b10) > 0 + + # Image stream request + if start_img_stream: + connection.send(bytes([3])) # Not implemented, skip + + # Sensor stream request + if start_sensors_stream: + # Calculate the time since the last data was received + current_time = time.time() + dt = current_time - last_update_time + last_update_time = current_time + + # Determine the number of steps the motors have moved based on the previously set speed + left_steps, right_steps = model.get_steps(dt) + + # Update the speed of the motors + model.update_speed(msg) + + # Create a sensor message and send it back, only motor steps implemented so far + sensor_msg = EpuckSensorPacket() + sensor_msg.motor_left_steps = left_steps + sensor_msg.motor_right_steps = right_steps + packed = sensor_msg.pack() + connection.sendall(packed) + + else: + print("No data from", client_address) + break + + except Exception as e: + print(f"Exception: {e}") + + finally: + # Clean up the connection + connection.close() + time.sleep(1) + print("Connection closed") + break + + +if __name__ == "__main__": + signal.signal(signal.SIGINT, lambda _: sock.close()) + arg_parser = argparse.ArgumentParser(description="Test server for e-puck driver") + arg_parser.add_argument( + "--ip", type=str, default="192.168.1.1", help="IP address of this server" + ) + arg_parser.add_argument( + "--port", type=int, default=10000, help="Port number of this server" + ) + args = arg_parser.parse_args() + main((args.ip, args.port)) diff --git a/src/epuck2_driver_cpp.cpp b/src/epuck2_driver_cpp.cpp index 77cccdd..51cb047 100644 --- a/src/epuck2_driver_cpp.cpp +++ b/src/epuck2_driver_cpp.cpp @@ -1,32 +1,49 @@ -#include +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "sensor_msgs/msg/range.hpp" +#include "std_msgs/msg/u_int8_multi_array.hpp" +#include +#include +#include +#include +#include #include -#include -#include -#include +#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -//#include -#include -#include -#include +#include +#include +/* clang-format off */ #define DEBUG_CONNECTION_INIT 1 #define DEBUG_ROS_PARAMS 1 #define DEBUG_UPDATE_SENSORS_DATA 0 @@ -42,1324 +59,1570 @@ #define READ_TIMEOUT_USEC 0 #define MAX_CONNECTION_TRIALS 3 -#define WHEEL_DIAMETER 4 // cm. -#define WHEEL_SEPARATION 5.3 // Separation between wheels (cm). -#define WHEEL_DISTANCE 0.053 // Distance between wheels in meters (axis length); it's the same value as "WHEEL_SEPARATION" but expressed in meters. -#define WHEEL_CIRCUMFERENCE ((WHEEL_DIAMETER*M_PI)/100.0) // Wheel circumference (meters). -#define MOT_STEP_DIST (WHEEL_CIRCUMFERENCE/1000.0) // Distance for each motor step (meters); a complete turn is 1000 steps (0.000125 meters per step (m/steps)). -#define ROBOT_RADIUS 0.035 // meters. - -#define LED_NUMBER 6 // total number of LEDs on the robot (0,2,4,6=leds, 8=body, 9=front) +#define WHEEL_DIAMETER 0.04 // m. +#define WHEEL_SEPARATION 0.053 // Separation between wheels, axis length (m). +#define WHEEL_CIRCUMFERENCE (WHEEL_DIAMETER * M_PI) // Wheel circumference (meters). +#define MOT_STEP_DIST (WHEEL_CIRCUMFERENCE / 1000.0) // Distance for each motor step (meters); a complete turn is 1000 steps (0.000125 meters per step (m/steps)). +#define ROBOT_RADIUS 0.035 // meters. +#define LED_NUMBER 6 // total number of LEDs on the robot (0,2,4,6=leds, 8=body, 9=front) #define RGB_LED_NUMBER 4 #define DEG2RAD(deg) (deg / 180 * M_PI) -#define GYRO_RAW2DPS (250.0/32768.0f) //250DPS (degrees per second) scale for int16 raw value +#define GYRO_RAW2DPS (250.0 / 32768.0f) // 250DPS (degrees per second) scale for int16 raw value #define STANDARD_GRAVITY 9.80665f -#define RESISTOR_R1 220 //kohm -#define RESISTOR_R2 330 //kohm -#define VOLTAGE_DIVIDER (1.0f * RESISTOR_R2 / (RESISTOR_R1 + RESISTOR_R2)) -#define VREF 3.0f //volt correspond to the voltage on the VREF+ pin -#define ADC_RESOLUTION 4096 -#define COEFF_ADC_TO_VOLT ((1.0f * ADC_RESOLUTION * VOLTAGE_DIVIDER) / VREF) //convertion from adc value to voltage -#define MAX_VOLTAGE 4.2f //volt -#define MIN_VOLTAGE 3.4f //volt - -// Communication variables -struct sockaddr_in robot_addr; -int fd; -unsigned char command[21]; -unsigned char header, sensor[104]; -int bytes_sent = 0, bytes_recv = 0; -bool camera_enabled, ground_sensors_enabled; -uint8_t expected_recv_packets = 0; -bool newImageReceived = false; -std::string epuckAddress(""); - - -// Sensors data variables -unsigned char image[160*120*2]; -float acceleration, orientation, inclination; /**< acceleration data*/ -int16_t accData[3]; -int16_t gyroRaw[3]; -float magneticField[3]; -uint8_t temperature; -int proxData[8]; /**< proximity sensors data*/ -int lightAvg; /**< light sensor data*/ -uint16_t distanceMm; -uint16_t micVolume[4]; /**< microphone data*/ -int16_t motorSteps[2]; -uint16_t batteryRaw; -uint8_t microSdState; -uint8_t irCheck, irAddress, irData; -uint8_t selector; -int16_t groundProx[3], groundAmbient[3]; -uint8_t buttonState; - - -// ROS variables -ros::Publisher proxPublisher[8]; -sensor_msgs::Range proxMsg[8]; -ros::Publisher laserPublisher; -sensor_msgs::LaserScan laserMsg; -ros::Publisher odomPublisher; -nav_msgs::Odometry odomMsg; -ros::Publisher imagePublisher; -ros::Publisher imuPublisher; -sensor_msgs::Imu imuMsg; -ros::Publisher microphonePublisher; -visualization_msgs::Marker microphoneMsg; -ros::Publisher floorPublisher; -visualization_msgs::Marker floorMsg; -ros::Publisher distSensPublisher; -sensor_msgs::Range distSensMsg; -ros::Publisher magFieldPublisher; -sensor_msgs::MagneticField magFieldMsg; -ros::Publisher magFieldVectorPublisher; -visualization_msgs::Marker magFieldVectorMsg; -ros::Publisher battPublisher; -sensor_msgs::BatteryState battMsg; - -ros::Subscriber cmdVelSubscriber, cmdLedSubscriber, cmdRgbLedsSubscriber; - -double leftStepsDiff = 0, rightStepsDiff = 0; -double leftStepsPrev = 0, rightStepsPrev = 0; -signed long int leftStepsRawPrev = 0, rightStepsRawPrev = 0; -signed long int motorPositionDataCorrect[2]; -double xPos, yPos, theta; -double deltaSteps, deltaTheta; -ros::Time currentTime, lastTime, currentTimeMap, lastTimeMap; -int overflowCountLeft = 0, overflowCountRight = 0; -int16_t gyroOffset[3] = {0, 0, 0}; // Used if making an initial calibration of the gyro. -int speedLeft = 0, speedRight = 0; - - -// General variables -std::string epuckname; - - -int initConnectionWithRobot() { - int ret_value; - std::stringstream ss; - struct timeval tv; - socklen_t len = sizeof(tv); - uint8_t trials = 0; - - robot_addr.sin_family = AF_INET; - robot_addr.sin_addr.s_addr = inet_addr(epuckAddress.c_str()); - robot_addr.sin_port = htons(1000); - - if(DEBUG_CONNECTION_INIT)fprintf(stderr, "Try to connect to %s:%d (TCP)\n", inet_ntoa(robot_addr.sin_addr), htons(robot_addr.sin_port)); - - fd = socket(AF_INET, SOCK_STREAM, 0); - if(fd < 0) { - perror("TCP cannot create socket: "); - return -1; - } - - // Set to non-blocking mode during connection otherwise it will block for too much time if the robot isn't ready to accept connections - if( (ret_value = fcntl(fd, F_GETFL, 0)) < 0) { - perror("Cannot get flag status: "); - return -1; - }// else { - // if((ret_value & O_NONBLOCK) > 0) { - // std::cout << "Non-blocking socket" << std::endl; - // } else { - // std::cout << "Blocking socket" << std::endl; - // } - //} - ret_value |= O_NONBLOCK; - if(fcntl(fd, F_SETFL, ret_value) < 0) { - perror("Cannot set non-blocking mode: "); - return -1; - } - - while(trials < MAX_CONNECTION_TRIALS) { - // Connection to the robot (server). - ret_value = connect(fd, (struct sockaddr *) &robot_addr, sizeof(robot_addr)); - if (ret_value == 0) { - break; - } else { - trials++; - if(DEBUG_CONNECTION_INIT)fprintf(stderr, "Connection trial %d\n", trials); - sleep(3); - } - } - - if(trials == MAX_CONNECTION_TRIALS) { - ss.str(""); - ss << "[" << epuckname << "] " << "Error, can't connect to tcp socket"; - //if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); - return -1; - } - - // Set to blocking mode. - if( (ret_value = fcntl(fd, F_GETFL, 0)) < 0) { - perror("Cannot get flag status: "); - return -1; - }// else { - // if((ret_value & O_NONBLOCK) > 0) { - // std::cout << "Non-blocking socket" << std::endl; - // } else { - // std::cout << "Blocking socket" << std::endl; - // } - //} - ret_value &= (~O_NONBLOCK); - if(fcntl(fd, F_SETFL, ret_value) < 0) { - perror("Cannot set blocking mode: "); - return -1; - } - - // Set the reception timeout. This is used when blocking mode is activated after connection. - tv.tv_sec = READ_TIMEOUT_SEC; - tv.tv_usec = READ_TIMEOUT_USEC; - ret_value = setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); - if(ret_value < 0) { - perror("Cannot set rx timeout: "); - return -1; - } - //ret_value = getsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, &len); - //if(ret_value < 0) { - // perror("Cannot read rx timeout: "); - //} else { - // std::cout << "rx timeout: " << tv.tv_sec << " s and " << tv.tv_usec << " us" << std::endl; - //} - - return 0; -} +#define RESISTOR_R1 220 // kohm +#define RESISTOR_R2 330 // kohm +#define VOLTAGE_DIVIDER (1.0f * RESISTOR_R2 / (RESISTOR_R1 + RESISTOR_R2)) +#define VREF 3.0f // volt correspond to the voltage on the VREF+ pin +#define ADC_RESOLUTION 4096 +#define COEFF_ADC_TO_VOLT ((1.0f * ADC_RESOLUTION * VOLTAGE_DIVIDER) / VREF) // convertion from adc value to voltage +#define MAX_VOLTAGE 4.2f // volt +#define MIN_VOLTAGE 3.4f // volt +/* clang-format on */ + +class EpuckDriver : public rclcpp::Node +{ +private: + // Communication variables + struct sockaddr_in robot_addr; + int fd; + unsigned char command[21]; + unsigned char header, sensor[104]; + int bytes_sent = 0, bytes_recv = 0; + bool camera_enabled, ground_sensors_enabled; + uint8_t expected_recv_packets = 0; + bool newImageReceived = false; + std::string epuckAddress; + uint16_t epuckPort; + + // Sensors data variables + unsigned char image[160 * 120 * 2]; + float acceleration, orientation, inclination; /**< acceleration data*/ + int16_t accData[3]; + int16_t gyroRaw[3]; + float magneticField[3]; + uint8_t temperature; + int proxData[8]; /**< proximity sensors data*/ + int lightAvg; /**< light sensor data*/ + uint16_t distanceMm; + uint16_t micVolume[4]; /**< microphone data*/ + int16_t motorSteps[2]; + uint16_t batteryRaw; + uint8_t microSdState; + uint8_t irCheck, irAddress, irData; + uint8_t selector; + int16_t groundProx[3], groundAmbient[3]; + uint8_t buttonState; + + // ROS variables + rclcpp::Publisher::SharedPtr proxPublisher[8]; + sensor_msgs::msg::Range proxMsg[8]; + rclcpp::Publisher::SharedPtr laserPublisher; + sensor_msgs::msg::LaserScan laserMsg; + rclcpp::Publisher::SharedPtr odomPublisher; + nav_msgs::msg::Odometry odomMsg; + image_transport::Publisher imagePublisher; + rclcpp::Publisher::SharedPtr imuPublisher; + sensor_msgs::msg::Imu imuMsg; + rclcpp::Publisher::SharedPtr microphonePublisher; + visualization_msgs::msg::Marker microphoneMsg; + rclcpp::Publisher::SharedPtr floorPublisher; + visualization_msgs::msg::Marker floorMsg; + rclcpp::Publisher::SharedPtr distSensPublisher; + sensor_msgs::msg::Range distSensMsg; + rclcpp::Publisher::SharedPtr magFieldPublisher; + sensor_msgs::msg::MagneticField magFieldMsg; + rclcpp::Publisher::SharedPtr magFieldVectorPublisher; + visualization_msgs::msg::Marker magFieldVectorMsg; + rclcpp::Publisher::SharedPtr battPublisher; + sensor_msgs::msg::BatteryState battMsg; + + rclcpp::Subscription::SharedPtr cmdVelSubscriber; + rclcpp::Subscription::SharedPtr cmdLedSubscriber; + rclcpp::Subscription::SharedPtr cmdRgbLedsSubscriber; + + std::shared_ptr br; + std::shared_ptr static_br; + + rclcpp::TimerBase::SharedPtr tf_timer; + rclcpp::Time odomLastPublished = rclcpp::Time(0); + + double leftStepsDiff = 0, rightStepsDiff = 0; + double leftStepsPrev = 0, rightStepsPrev = 0; + signed long int leftStepsRawPrev = 0, rightStepsRawPrev = 0; + signed long int motorPositionDataCorrect[2]; + double xPos, yPos, theta; + double deltaSteps, deltaTheta; + rclcpp::Time currentTime, lastTime, currentTimeMap, lastTimeMap; + int overflowCountLeft = 0, overflowCountRight = 0; + int16_t gyroOffset[3] = {0, 0, 0}; // Used if making an initial calibration of the gyro. + int speedLeft = 0, speedRight = 0; + + // General variables + std::string epuckname; + +public: + EpuckDriver() : rclcpp::Node("epuck_driver_cpp") + { + this->declare_parameter("epuck2_id"); + this->declare_parameter("epuck2_address"); + this->declare_parameter("epuck2_port"); + this->declare_parameter("epuck2_name"); + this->declare_parameter("robot/xpos", 0.0); + this->declare_parameter("robot/ypos", 0.0); + this->declare_parameter("robot/theta", 0.0); + this->declare_parameter("robot/camera", false); + this->declare_parameter("robot/floor", false); + + br = std::make_shared(this); + static_br = std::make_shared(this); + } -void closeConnection() { - std::stringstream ss; - if(close(fd) < 0) { - ss.str(""); - ss << "[" << epuckname << "] " << "Can't close tcp socket"; - if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); + ~EpuckDriver() override { closeConnection(); } + + int initConnectionWithRobot() + { + int ret_value; + std::stringstream ss; + struct timeval tv; + socklen_t len = sizeof(tv); + uint8_t trials = 0; + + robot_addr.sin_family = AF_INET; + robot_addr.sin_addr.s_addr = inet_addr(epuckAddress.c_str()); + robot_addr.sin_port = htons(epuckPort); + + if (DEBUG_CONNECTION_INIT) + fprintf(stderr, "Try to connect to %s:%d (TCP)\n", inet_ntoa(robot_addr.sin_addr), + htons(robot_addr.sin_port)); + + fd = socket(AF_INET, SOCK_STREAM, 0); + if (fd < 0) + { + perror("TCP cannot create socket: "); + return -1; + } + + // Set to non-blocking mode during connection otherwise it will block for too much time if the robot isn't ready + // to accept connections + if ((ret_value = fcntl(fd, F_GETFL, 0)) < 0) + { + perror("Cannot get flag status: "); + return -1; + } // else { + // if((ret_value & O_NONBLOCK) > 0) { + // std::cout << "Non-blocking socket" << std::endl; + // } else { + // std::cout << "Blocking socket" << std::endl; + // } + //} + ret_value |= O_NONBLOCK; + if (fcntl(fd, F_SETFL, ret_value) < 0) + { + perror("Cannot set non-blocking mode: "); + return -1; + } + + while (trials < MAX_CONNECTION_TRIALS) + { + // Connection to the robot (server). + ret_value = connect(fd, (struct sockaddr *)&robot_addr, sizeof(robot_addr)); + if (ret_value == 0) + { + break; + } else + { + trials++; + if (DEBUG_CONNECTION_INIT) fprintf(stderr, "Connection trial %d\n", trials); + sleep(3); + } + } + + if (trials == MAX_CONNECTION_TRIALS) + { + ss.str(""); + ss << "[" << epuckname << "] " << "Error, can't connect to tcp socket"; + // if(DEBUG_CONNECTION_INIT)perror(ss.str().c_str()); + return -1; + } + + // Set to blocking mode. + if ((ret_value = fcntl(fd, F_GETFL, 0)) < 0) + { + perror("Cannot get flag status: "); + return -1; + } // else { + // if((ret_value & O_NONBLOCK) > 0) { + // std::cout << "Non-blocking socket" << std::endl; + // } else { + // std::cout << "Blocking socket" << std::endl; + // } + //} + ret_value &= (~O_NONBLOCK); + if (fcntl(fd, F_SETFL, ret_value) < 0) + { + perror("Cannot set blocking mode: "); + return -1; + } + + // Set the reception timeout. This is used when blocking mode is activated after connection. + tv.tv_sec = READ_TIMEOUT_SEC; + tv.tv_usec = READ_TIMEOUT_USEC; + ret_value = setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + if (ret_value < 0) + { + perror("Cannot set rx timeout: "); + return -1; + } + // ret_value = getsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, &len); + // if(ret_value < 0) { + // perror("Cannot read rx timeout: "); + // } else { + // std::cout << "rx timeout: " << tv.tv_sec << " s and " << tv.tv_usec << " us" << std::endl; + // } + + return 0; } -} -void updateSensorsAndActuators() { - int bytes_sent = 0, bytes_recv = 0, ret_value; - long mantis = 0; - short exp = 0; - float flt = 0.0; - - bytes_sent = 0; - while(bytes_sent < sizeof(command)) { - bytes_sent += send(fd, (char *)&command[bytes_sent], sizeof(command)-bytes_sent, 0); - } - command[2] = 0; // Stop proximity calibration. - - while(expected_recv_packets > 0) { - bytes_recv = recv(fd, (char *)&header, 1, 0); - if (bytes_recv <= 0) { - closeConnection(); - if(initConnectionWithRobot() < 0) { - std::cerr << "Lost connection with the robot" << std::endl; - exit(1); - } else { - return; // Wait for the next sensor request - } - } - - switch(header) { - case 0x01: // Camera. - bytes_recv = 0; - while(bytes_recv < sizeof(image)) { - ret_value = recv(fd, (char *)&image[bytes_recv], sizeof(image)-bytes_recv, 0); - if(ret_value <= 0) { - closeConnection(); - if(initConnectionWithRobot() < 0) { - std::cerr << "Lost connection with the robot" << std::endl; - exit(1); - } else { - return; // Wait for the next sensor request - } - } else { - bytes_recv += ret_value; - //std::cout << "image read = " << bytes_recv << std::endl; - } - } - - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "camera read correctly" << std::endl; - newImageReceived = true; - - //red = image[0] & 0xf8; - //green = image[0] << 5; - //green += (image[1] & 0xf8) >> 3; - //blue = image[1] << 3; - //printf("1st pixel = %d, %d, %d\r\n", red, green, blue); - break; - - case 0x02: // Sensors. - bytes_recv = 0; - while(bytes_recv < sizeof(sensor)) { - ret_value = recv(fd, (char *)&sensor[bytes_recv], sizeof(sensor)-bytes_recv, 0); - if(ret_value <= 0) { - closeConnection(); - if(initConnectionWithRobot() < 0) { - std::cerr << "Lost connection with the robot" << std::endl; - exit(1); - } else { - return; // Wait for the next sensor request - } - } else { - bytes_recv += ret_value; - //std::cout << "sensors read = " << bytes_recv << std::endl; - } - } - - accData[0] = sensor[0] + sensor[1]*256; - accData[1] = sensor[2] + sensor[3]*256; - accData[2] = sensor[4] + sensor[5]*256; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "acc: " << accData[0] << "," << accData[1] << "," << accData[2] << std::endl; - - // Compute acceleration - mantis = (sensor[6] & 0xff) + ((sensor[7] & 0xffl) << 8) + (((sensor[8] &0x7fl) | 0x80) << 16); - exp = (sensor[9] & 0x7f) * 2 + ((sensor[8] & 0x80) ? 1 : 0); - if (sensor[9] & 0x80) { - mantis = -mantis; - } - flt = (mantis || exp) ? ((float) ldexp (mantis, (exp - 127 - 23))): 0; - acceleration=flt; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "acceleration: " << acceleration << std::endl; - - // Compute orientation. - mantis = (sensor[10] & 0xff) + ((sensor[11] & 0xffl) << 8) + (((sensor[12] &0x7fl) | 0x80) << 16); - exp = (sensor[13] & 0x7f) * 2 + ((sensor[12] & 0x80) ? 1 : 0); - if (sensor[13] & 0x80) - mantis = -mantis; - flt = (mantis || exp) ? ((float) ldexp (mantis, (exp - 127 - 23))): 0; - orientation=flt; - if (orientation < 0.0 ) - orientation=0.0; - if (orientation > 360.0 ) - orientation=360.0; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "orientation: " << orientation << std::endl; - - // Compute inclination. - mantis = (sensor[14] & 0xff) + ((sensor[15] & 0xffl) << 8) + (((sensor[16] &0x7fl) | 0x80) << 16); - exp = (sensor[17] & 0x7f) * 2 + ((sensor[16] & 0x80) ? 1 : 0); - if (sensor[17] & 0x80) - mantis = -mantis; - flt = (mantis || exp) ? ((float) ldexp (mantis, (exp - 127 - 23))): 0; - inclination=flt; - if (inclination < 0.0 ) - inclination=0.0; - if (inclination > 180.0 ) - inclination=180.0; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "inclination: " << inclination << std::endl; - - // Gyro - gyroRaw[0] = sensor[18]+sensor[19]*256; - gyroRaw[1] = sensor[20]+sensor[21]*256; - gyroRaw[2] = sensor[22]+sensor[23]*256; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "gyro: " << gyroRaw[0] << "," << gyroRaw[1] << "," << gyroRaw[2] << std::endl; - - // Magnetometer - magneticField[0] = *((float*)&sensor[24]); - magneticField[1] = *((float*)&sensor[28]); - magneticField[2] = *((float*)&sensor[32]); - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "mag: " << magneticField[0] << "," << magneticField[1] << "," << magneticField[2] << std::endl; - - // Temperature. - temperature = sensor[36]; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "temperature: " << (int)temperature << std::endl; - - // Proximity sensors data. - proxData[0] = sensor[37]+sensor[38]*256; - proxData[1] = sensor[39]+sensor[40]*256; - proxData[2] = sensor[41]+sensor[42]*256; - proxData[3] = sensor[43]+sensor[44]*256; - proxData[4] = sensor[45]+sensor[46]*256; - proxData[5] = sensor[47]+sensor[48]*256; - proxData[6] = sensor[49]+sensor[50]*256; - proxData[7] = sensor[51]+sensor[52]*256; - if(proxData[0]<0) { - proxData[0]=0; - } - if(proxData[1]<0) { - proxData[1]=0; - } - if(proxData[2]<0) { - proxData[2]=0; - } - if(proxData[3]<0) { - proxData[3]=0; - } - if(proxData[4]<0) { - proxData[4]=0; - } - if(proxData[5]<0) { - proxData[5]=0; - } - if(proxData[6]<0) { - proxData[6]=0; - } - if(proxData[7]<0) { - proxData[7]=0; - } - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "prox: " << proxData[0] << "," << proxData[1] << "," << proxData[2] << "," << proxData[3] << "," << proxData[4] << "," << proxData[5] << "," << proxData[6] << "," << proxData[7] << std::endl; - - // Compute abmient light. - lightAvg += (sensor[53]+sensor[54]*256); - lightAvg += (sensor[55]+sensor[56]*256); - lightAvg += (sensor[57]+sensor[58]*256); - lightAvg += (sensor[59]+sensor[60]*256); - lightAvg += (sensor[61]+sensor[62]*256); - lightAvg += (sensor[63]+sensor[64]*256); - lightAvg += (sensor[65]+sensor[66]*256); - lightAvg += (sensor[67]+sensor[68]*256); - lightAvg = (int) (lightAvg/8); - lightAvg = (lightAvg>4000)?4000:lightAvg; - if(lightAvg<0) { - lightAvg=0; - } - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "lightAvg: " << lightAvg << std::endl; - - // ToF - distanceMm = (uint16_t)((uint8_t)sensor[70]<<8)|((uint8_t)sensor[69]); - if(distanceMm > 2000) { - distanceMm = 2000; - } - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "distanceMm: " << distanceMm << "(" << (int)sensor[69] << "," << (int)sensor[70] << ")" << std::endl; - - // Microphone - micVolume[0] = ((uint8_t)sensor[71]+(uint8_t)sensor[72]*256); - micVolume[1] = ((uint8_t)sensor[73]+(uint8_t)sensor[74]*256); - micVolume[2] = ((uint8_t)sensor[75]+(uint8_t)sensor[76]*256); - micVolume[3] = ((uint8_t)sensor[77]+(uint8_t)sensor[78]*256); - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "mic: " << micVolume[0] << "," << micVolume[1] << "," << micVolume[2] << "," << micVolume[3] << std::endl; - - // Left steps - motorSteps[0] = (sensor[79]+sensor[80]*256); - // Right steps - motorSteps[1] = (sensor[81]+sensor[82]*256); - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "steps: " << motorSteps[0] << "," << motorSteps[1] << std::endl; - - // Battery - batteryRaw = (uint8_t)sensor[83]+(uint8_t)sensor[84]*256; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "batteryRaw: " << batteryRaw << std::endl; - - // Micro sd state. - microSdState = sensor[85]; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "microSdState: " << (int)microSdState << std::endl; - - // Tv remote. - irCheck = sensor[86]; - irAddress = sensor[87]; - irData = sensor[88]; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "tv remote: " << (int)irCheck << "," << (int)irAddress << "," << (int)irData << std::endl; - - // Selector. - selector = sensor[89]; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "selector: " << (int)selector << std::endl; - - // Ground sensor proximity. - groundProx[0] = sensor[90]+sensor[91]*256; - groundProx[1] = sensor[92]+sensor[93]*256; - groundProx[2] = sensor[94]+sensor[95]*256; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "groundProx: " << groundProx[0] << "," << groundProx[1] << "," << groundProx[2] << std::endl; - - // Ground sensor ambient light. - groundAmbient[0] = sensor[96]+sensor[97]*256; - groundAmbient[1] = sensor[98]+sensor[99]*256; - groundAmbient[2] = sensor[100]+sensor[101]*256; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "groundAmbient: " << groundAmbient[0] << "," << groundAmbient[1] << "," << groundAmbient[2] << std::endl; - - // Button state. - buttonState = sensor[102]; - if(DEBUG_UPDATE_SENSORS_DATA)std::cout << "[" << epuckname << "] " << "buttonState: " << (int)buttonState << std::endl; - - break; - - case 0x03: - printf("empty packet\r\n"); - break; - - default: - printf("unexpected packet\r\n"); - break; - } - expected_recv_packets--; - } - - if(camera_enabled) { - expected_recv_packets = 2; - } else { - expected_recv_packets = 1; - } - -} + void closeConnection() + { + std::stringstream ss; + if (close(fd) < 0) + { + ss.str(""); + ss << "[" << epuckname << "] " << "Can't close tcp socket"; + if (DEBUG_CONNECTION_INIT) perror(ss.str().c_str()); + } + } -void RGB565toRGB888(int width, int height, unsigned char *src, unsigned char *dst) { - int line, column; - int index_src=0, index_dst=0; + void updateSensorsAndActuators() + { + int bytes_sent = 0, bytes_recv = 0, ret_value; + long mantis = 0; + short exp = 0; + float flt = 0.0; + + bytes_sent = 0; + while (bytes_sent < sizeof(command)) + { + bytes_sent += send(fd, (char *)&command[bytes_sent], sizeof(command) - bytes_sent, 0); + } + command[2] = 0; // Stop proximity calibration. + + while (expected_recv_packets > 0) + { + bytes_recv = recv(fd, (char *)&header, 1, 0); + if (bytes_recv <= 0) + { + closeConnection(); + if (initConnectionWithRobot() < 0) + { + std::cerr << "Lost connection with the robot" << std::endl; + exit(1); + } else + { + return; // Wait for the next sensor request + } + } + + switch (header) + { + case 0x01: // Camera. + bytes_recv = 0; + while (bytes_recv < sizeof(image)) + { + ret_value = recv(fd, (char *)&image[bytes_recv], sizeof(image) - bytes_recv, 0); + if (ret_value <= 0) + { + closeConnection(); + if (initConnectionWithRobot() < 0) + { + std::cerr << "Lost connection with the robot" << std::endl; + exit(1); + } else + { + return; // Wait for the next sensor request + } + } else + { + bytes_recv += ret_value; + // std::cout << "image read = " << bytes_recv << std::endl; + } + } + + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "camera read correctly" << std::endl; + newImageReceived = true; + + // red = image[0] & 0xf8; + // green = image[0] << 5; + // green += (image[1] & 0xf8) >> 3; + // blue = image[1] << 3; + // printf("1st pixel = %d, %d, %d\r\n", red, green, blue); + break; + + case 0x02: // Sensors. + bytes_recv = 0; + while (bytes_recv < sizeof(sensor)) + { + ret_value = recv(fd, (char *)&sensor[bytes_recv], sizeof(sensor) - bytes_recv, 0); + if (ret_value <= 0) + { + closeConnection(); + if (initConnectionWithRobot() < 0) + { + std::cerr << "Lost connection with the robot" << std::endl; + exit(1); + } else + { + return; // Wait for the next sensor request + } + } else + { + bytes_recv += ret_value; + // std::cout << "sensors read = " << bytes_recv << std::endl; + } + } + + accData[0] = sensor[0] + sensor[1] * 256; + accData[1] = sensor[2] + sensor[3] * 256; + accData[2] = sensor[4] + sensor[5] * 256; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "acc: " << accData[0] << "," << accData[1] << "," + << accData[2] << std::endl; + + // Compute acceleration + mantis = (sensor[6] & 0xff) + ((sensor[7] & 0xffl) << 8) + (((sensor[8] & 0x7fl) | 0x80) << 16); + exp = (sensor[9] & 0x7f) * 2 + ((sensor[8] & 0x80) ? 1 : 0); + if (sensor[9] & 0x80) { mantis = -mantis; } + flt = (mantis || exp) ? ((float)ldexp(mantis, (exp - 127 - 23))) : 0; + acceleration = flt; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "acceleration: " << acceleration << std::endl; + + // Compute orientation. + mantis = (sensor[10] & 0xff) + ((sensor[11] & 0xffl) << 8) + (((sensor[12] & 0x7fl) | 0x80) << 16); + exp = (sensor[13] & 0x7f) * 2 + ((sensor[12] & 0x80) ? 1 : 0); + if (sensor[13] & 0x80) mantis = -mantis; + flt = (mantis || exp) ? ((float)ldexp(mantis, (exp - 127 - 23))) : 0; + orientation = flt; + if (orientation < 0.0) orientation = 0.0; + if (orientation > 360.0) orientation = 360.0; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "orientation: " << orientation << std::endl; + + // Compute inclination. + mantis = (sensor[14] & 0xff) + ((sensor[15] & 0xffl) << 8) + (((sensor[16] & 0x7fl) | 0x80) << 16); + exp = (sensor[17] & 0x7f) * 2 + ((sensor[16] & 0x80) ? 1 : 0); + if (sensor[17] & 0x80) mantis = -mantis; + flt = (mantis || exp) ? ((float)ldexp(mantis, (exp - 127 - 23))) : 0; + inclination = flt; + if (inclination < 0.0) inclination = 0.0; + if (inclination > 180.0) inclination = 180.0; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "inclination: " << inclination << std::endl; + + // Gyro + gyroRaw[0] = sensor[18] + sensor[19] * 256; + gyroRaw[1] = sensor[20] + sensor[21] * 256; + gyroRaw[2] = sensor[22] + sensor[23] * 256; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "gyro: " << gyroRaw[0] << "," << gyroRaw[1] << "," + << gyroRaw[2] << std::endl; + + // Magnetometer + magneticField[0] = *((float *)&sensor[24]); + magneticField[1] = *((float *)&sensor[28]); + magneticField[2] = *((float *)&sensor[32]); + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "mag: " << magneticField[0] << "," << magneticField[1] + << "," << magneticField[2] << std::endl; + + // Temperature. + temperature = sensor[36]; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "temperature: " << (int)temperature << std::endl; + + // Proximity sensors data. + proxData[0] = sensor[37] + sensor[38] * 256; + proxData[1] = sensor[39] + sensor[40] * 256; + proxData[2] = sensor[41] + sensor[42] * 256; + proxData[3] = sensor[43] + sensor[44] * 256; + proxData[4] = sensor[45] + sensor[46] * 256; + proxData[5] = sensor[47] + sensor[48] * 256; + proxData[6] = sensor[49] + sensor[50] * 256; + proxData[7] = sensor[51] + sensor[52] * 256; + if (proxData[0] < 0) { proxData[0] = 0; } + if (proxData[1] < 0) { proxData[1] = 0; } + if (proxData[2] < 0) { proxData[2] = 0; } + if (proxData[3] < 0) { proxData[3] = 0; } + if (proxData[4] < 0) { proxData[4] = 0; } + if (proxData[5] < 0) { proxData[5] = 0; } + if (proxData[6] < 0) { proxData[6] = 0; } + if (proxData[7] < 0) { proxData[7] = 0; } + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "prox: " << proxData[0] << "," << proxData[1] << "," + << proxData[2] << "," << proxData[3] << "," << proxData[4] << "," << proxData[5] << "," + << proxData[6] << "," << proxData[7] << std::endl; + + // Compute abmient light. + lightAvg += (sensor[53] + sensor[54] * 256); + lightAvg += (sensor[55] + sensor[56] * 256); + lightAvg += (sensor[57] + sensor[58] * 256); + lightAvg += (sensor[59] + sensor[60] * 256); + lightAvg += (sensor[61] + sensor[62] * 256); + lightAvg += (sensor[63] + sensor[64] * 256); + lightAvg += (sensor[65] + sensor[66] * 256); + lightAvg += (sensor[67] + sensor[68] * 256); + lightAvg = (int)(lightAvg / 8); + lightAvg = (lightAvg > 4000) ? 4000 : lightAvg; + if (lightAvg < 0) { lightAvg = 0; } + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "lightAvg: " << lightAvg << std::endl; + + // ToF + distanceMm = (uint16_t)((uint8_t)sensor[70] << 8) | ((uint8_t)sensor[69]); + if (distanceMm > 2000) { distanceMm = 2000; } + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "distanceMm: " << distanceMm << "(" << (int)sensor[69] + << "," << (int)sensor[70] << ")" << std::endl; + + // Microphone + micVolume[0] = ((uint8_t)sensor[71] + (uint8_t)sensor[72] * 256); + micVolume[1] = ((uint8_t)sensor[73] + (uint8_t)sensor[74] * 256); + micVolume[2] = ((uint8_t)sensor[75] + (uint8_t)sensor[76] * 256); + micVolume[3] = ((uint8_t)sensor[77] + (uint8_t)sensor[78] * 256); + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "mic: " << micVolume[0] << "," << micVolume[1] << "," + << micVolume[2] << "," << micVolume[3] << std::endl; + + // Left steps + motorSteps[0] = (sensor[79] + sensor[80] * 256); + // Right steps + motorSteps[1] = (sensor[81] + sensor[82] * 256); + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "steps: " << motorSteps[0] << "," << motorSteps[1] + << std::endl; + + // Battery + batteryRaw = (uint8_t)sensor[83] + (uint8_t)sensor[84] * 256; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "batteryRaw: " << batteryRaw << std::endl; + + // Micro sd state. + microSdState = sensor[85]; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "microSdState: " << (int)microSdState << std::endl; + + // Tv remote. + irCheck = sensor[86]; + irAddress = sensor[87]; + irData = sensor[88]; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "tv remote: " << (int)irCheck << "," << (int)irAddress + << "," << (int)irData << std::endl; + + // Selector. + selector = sensor[89]; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "selector: " << (int)selector << std::endl; + + // Ground sensor proximity. + groundProx[0] = sensor[90] + sensor[91] * 256; + groundProx[1] = sensor[92] + sensor[93] * 256; + groundProx[2] = sensor[94] + sensor[95] * 256; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "groundProx: " << groundProx[0] << "," << groundProx[1] + << "," << groundProx[2] << std::endl; + + // Ground sensor ambient light. + groundAmbient[0] = sensor[96] + sensor[97] * 256; + groundAmbient[1] = sensor[98] + sensor[99] * 256; + groundAmbient[2] = sensor[100] + sensor[101] * 256; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "groundAmbient: " << groundAmbient[0] << "," + << groundAmbient[1] << "," << groundAmbient[2] << std::endl; + + // Button state. + buttonState = sensor[102]; + if (DEBUG_UPDATE_SENSORS_DATA) + std::cout << "[" << epuckname << "] " << "buttonState: " << (int)buttonState << std::endl; + + break; + + case 0x03: printf("empty packet\r\n"); break; + + default: printf("unexpected packet\r\n"); break; + } + expected_recv_packets--; + } - for (line = 0; line < height; ++line) { - for (column = 0; column < width; ++column) { - dst[index_dst++] = (unsigned char)(src[index_src] & 0xF8); - dst[index_dst++] = (unsigned char)((src[index_src]&0x07)<<5) | (unsigned char)((src[index_src+1]&0xE0)>>3); - dst[index_dst++] = (unsigned char)((src[index_src+1]&0x1F)<<3); - index_src+=2; + if (camera_enabled) + { + expected_recv_packets = 2; + } else + { + expected_recv_packets = 1; } } -} -void updateRosInfo() { - static tf::TransformBroadcaster br; - std::stringstream ss; - geometry_msgs::Quaternion orientQuat; - std::stringstream parent; - std::stringstream child; - tf::Transform transform; - tf::Quaternion q; - - int i = 0; - - //############################################################################################################################################# - // Proximity topics - for(i=0; i<8; i++) { - if(proxData[i] > 0) { - proxMsg[i].range = 0.5/sqrt(proxData[i]); // Transform the analog value to a distance value in meters (given from field tests). - } else { - proxMsg[i].range = proxMsg[i].max_range; - } - if(proxMsg[i].range > proxMsg[i].max_range) { - proxMsg[i].range = proxMsg[i].max_range; - } - if(proxMsg[i].range < proxMsg[i].min_range) { - proxMsg[i].range = proxMsg[i].min_range; - } - proxMsg[i].header.stamp = ros::Time::now(); - proxPublisher[i].publish(proxMsg[i]); - } - - // e-puck proximity positions (cm), x pointing forward, y pointing left - // P7(3.5, 1.0) P0(3.5, -1.0) - // P6(2.5, 2.5) P1(2.5, -2.5) - // P5(0.0, 3.0) P2(0.0, -3.0) - // P4(-3.5, 2.0) P3(-3.5, -2.0) - // - // e-puck proximity orentations (degrees) - // P7(10) P0(350) - // P6(40) P1(320) - // P5(90) P2(270) - // P4(160) P3(200) - - transform.setOrigin( tf::Vector3(0.035, -0.010, 0.034) ); - q.setRPY(0, 0, 6.11); - transform.setRotation(q); - parent << epuckname << "/base_prox0"; - child << epuckname << "/base_link"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - - transform.setOrigin( tf::Vector3(0.025, -0.025, 0.034) ); - q.setRPY(0, 0, 5.59); - transform.setRotation(q); - parent.str(""); - parent << epuckname << "/base_prox1"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - - transform.setOrigin( tf::Vector3(0.000, -0.030, 0.034) ); - q.setRPY(0, 0, 4.71); - transform.setRotation(q); - parent.str(""); - parent << epuckname << "/base_prox2"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - - transform.setOrigin( tf::Vector3(-0.035, -0.020, 0.034) ); - q.setRPY(0, 0, 3.49); - transform.setRotation(q); - parent.str(""); - parent << epuckname << "/base_prox3"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - - transform.setOrigin( tf::Vector3(-0.035, 0.020, 0.034) ); - q.setRPY(0, 0, 2.8); - transform.setRotation(q); - parent.str(""); - parent << epuckname << "/base_prox4"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - - transform.setOrigin( tf::Vector3(0.000, 0.030, 0.034) ); - q.setRPY(0, 0, 1.57); - transform.setRotation(q); - parent.str(""); - parent << epuckname << "/base_prox5"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - - transform.setOrigin( tf::Vector3(0.025, 0.025, 0.034) ); - q.setRPY(0, 0, 0.70); - transform.setRotation(q); - parent.str(""); - parent << epuckname << "/base_prox6"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - - transform.setOrigin( tf::Vector3(0.035, 0.010, 0.034) ); - q.setRPY(0, 0, 0.17); - transform.setRotation(q); - parent.str(""); - parent << epuckname << "/base_prox7"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - //############################################################################################################################################# - - //############################################################################################################################################# - // Laserscan topic - currentTimeMap = ros::Time::now(); - parent.str(""); - parent << epuckname << "/base_laser"; - //populate the LaserScan message - laserMsg.header.stamp = ros::Time::now(); - laserMsg.header.frame_id = parent.str(); - laserMsg.angle_min = -M_PI/2.0; - laserMsg.angle_max = M_PI/2.0; - laserMsg.angle_increment = M_PI/18.0; // 10 degrees. - //laserMsg.time_increment = (currentTimeMap-lastTimeMap).toSec()/180; //0.003; //(1 / laser_frequency) / (num_readings); - //laserMsg.scan_time = (currentTimeMap-lastTimeMap).toSec(); - // The laser is placed in the center of the robot, but the proximity sensors are placed around the robot thus add "ROBOT_RADIUS" to get correct values. - laserMsg.range_min = 0.005+ROBOT_RADIUS; // 0.5 cm + ROBOT_RADIUS. - laserMsg.range_max = 0.05+ROBOT_RADIUS; // 5 cm + ROBOT_RADIUS. - laserMsg.ranges.resize(19); - laserMsg.intensities.resize(19); - lastTimeMap = ros::Time::now(); - - // We use the information from the 6 proximity sensors on the front side of the robot to get 19 laser scan points. The interpolation used is the following: - // -90 degrees: P2 - // -80 degrees: 4/5*P2 + 1/5*P1 - // -70 degrees: 3/5*P2 + 2/5*P1 - // -60 degrees: 2/5*P2 + 3/5*P1 - // -50 degrees: 1/5*P2 + 4/5*P1 - // -40 degrees: P1 - // -30 degrees: 2/3*P1 + 1/3*P0 - // -20 degrees: 1/3*P1 + 2/3*P0 - // -10 degrees: P0 - // 0 degrees: 1/2*P0 + 1/2*P7 - // 10 degrees: P7 - // 20 degrees: 1/3*P6 + 2/3*P7 - // 30 degrees: 2/3*P6 + 1/3*P7 - // 40 degrees: P6 - // 50 degrees: 1/5*P5 + 4/5*P6 - // 60 degrees: 2/5*P5 + 3/5*P6 - // 70 degrees: 3/5*P5 + 2/5*P6 - // 80 degrees: 4/5*P5 + 1/5*P6 - // 90 degrees: P5 - - float tempProx; - tempProx = proxData[2]; - if(tempProx > 0) { - laserMsg.ranges[0] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[0] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[0] = laserMsg.range_max; - laserMsg.intensities[0] = 0; - } - - tempProx = proxData[2]*4/5 + proxData[1]*1/5; - if(tempProx > 0) { - laserMsg.ranges[1] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[1] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[1] = laserMsg.range_max; - laserMsg.intensities[1] = 0; - } - - tempProx = proxData[2]*3/5 + proxData[1]*2/5; - if(tempProx > 0) { - laserMsg.ranges[2] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[2] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[2] = laserMsg.range_max; - laserMsg.intensities[2] = 0; - } - - tempProx = proxData[2]*2/5 + proxData[1]*3/5; - if(tempProx > 0) { - laserMsg.ranges[3] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[3] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[3] = laserMsg.range_max; - laserMsg.intensities[3] = 0; - } - - tempProx = proxData[2]*1/5 + proxData[1]*4/5; - if(tempProx > 0) { - laserMsg.ranges[4] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[4] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[4] = laserMsg.range_max; - laserMsg.intensities[4] = 0; - } - - tempProx = proxData[1]; - if(tempProx > 0) { - laserMsg.ranges[5] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[5] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[5] = laserMsg.range_max; - laserMsg.intensities[5] = 0; - } - - tempProx = proxData[1]*2/3 + proxData[0]*1/3; - if(tempProx > 0) { - laserMsg.ranges[6] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[6] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[6] = laserMsg.range_max; - laserMsg.intensities[6] = 0; - } - - tempProx = proxData[1]*1/3 + proxData[0]*2/3; - if(tempProx > 0) { - laserMsg.ranges[7] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[7] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[7] = laserMsg.range_max; - laserMsg.intensities[7] = 0; - } - - tempProx = proxData[0]; - if(tempProx > 0) { - laserMsg.ranges[8] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[8] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[8] = laserMsg.range_max; - laserMsg.intensities[8] = 0; - } - - tempProx = (proxData[0]+proxData[7])>>1; - if(tempProx > 0) { - laserMsg.ranges[9] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[9] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[9] = laserMsg.range_max; - laserMsg.intensities[9] = 0; - } - - tempProx = proxData[7]; - if(tempProx > 0) { - laserMsg.ranges[10] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[10] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[10] = laserMsg.range_max; - laserMsg.intensities[10] = 0; - } - - tempProx = proxData[7]*2/3 + proxData[6]*1/3; - if(tempProx > 0) { - laserMsg.ranges[11] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[11] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[11] = laserMsg.range_max; - laserMsg.intensities[11] = 0; - } - - tempProx = proxData[7]*1/3 + proxData[6]*2/3; - if(tempProx > 0) { - laserMsg.ranges[12] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[12] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[12] = laserMsg.range_max; - laserMsg.intensities[12] = 0; - } - - tempProx = proxData[6]; - if(tempProx > 0) { - laserMsg.ranges[13] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[13] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[13] = laserMsg.range_max; - laserMsg.intensities[13] = 0; - } - - tempProx = proxData[6]*4/5 + proxData[5]*1/5; - if(tempProx > 0) { - laserMsg.ranges[14] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[14] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[14] = laserMsg.range_max; - laserMsg.intensities[14] = 0; - } - - tempProx = proxData[6]*3/5 + proxData[5]*2/5; - if(tempProx > 0) { - laserMsg.ranges[15] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[15] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[15] = laserMsg.range_max; - laserMsg.intensities[15] = 0; - } - - tempProx = proxData[6]*2/5 + proxData[5]*3/5; - if(tempProx > 0) { - laserMsg.ranges[16] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[16] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[16] = laserMsg.range_max; - laserMsg.intensities[16] = 0; - } - - tempProx = proxData[6]*1/5 + proxData[5]*4/5; - if(tempProx > 0) { - laserMsg.ranges[17] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[17] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[17] = laserMsg.range_max; - laserMsg.intensities[17] = 0; - } - - tempProx = proxData[5]; - if(tempProx > 0) { - laserMsg.ranges[18] = (0.5/sqrt(tempProx))+ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). - laserMsg.intensities[18] = tempProx; - } else { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. - laserMsg.ranges[18] = laserMsg.range_max; - laserMsg.intensities[18] = 0; - } - - for(i=0; i<19; i++) { - if(laserMsg.ranges[i] > laserMsg.range_max) { - laserMsg.ranges[i] = laserMsg.range_max; - } - if(laserMsg.ranges[i] < laserMsg.range_min) { - laserMsg.ranges[i] = laserMsg.range_min; - } - } - - transform.setOrigin( tf::Vector3(0.0, 0.0, 0.034) ); - q.setRPY(0, 0, 0); - transform.setRotation(q); - parent.str(""); - child.str(""); - parent << epuckname << "/base_laser"; - child << epuckname << "/base_link"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - laserPublisher.publish(laserMsg); - //############################################################################################################################################# - - //############################################################################################################################################# - // Odometry topic - // The encoders values coming from the e-puck are 2 bytes signed int thus we need to handle the overflows otherwise the odometry will be wrong after a while (about 4 meters). - if((leftStepsRawPrev>0) && (motorSteps[0]<0) && (abs(motorSteps[0]-leftStepsRawPrev)>30000)) { // Overflow detected (positive). - overflowCountLeft++; - } - if((leftStepsRawPrev<0) && (motorSteps[0]>0) && (abs(motorSteps[0]-leftStepsRawPrev)>30000)) { // Overflow detected (negative). - overflowCountLeft--; - } - motorPositionDataCorrect[0] = (overflowCountLeft*65536) + motorSteps[0]; - - if((rightStepsRawPrev>0) && (motorSteps[1]<0) && (abs(motorSteps[1]-rightStepsRawPrev)>30000)) { // Overflow detected (positive). - overflowCountRight++; - } - if((rightStepsRawPrev<0) && (motorSteps[1]>0) && (abs(motorSteps[1]-rightStepsRawPrev)>30000)) { // Overflow detected (negative). - overflowCountRight--; - } - motorPositionDataCorrect[1] = (overflowCountRight*65536) + motorSteps[1]; - - leftStepsRawPrev = motorSteps[0]; - rightStepsRawPrev = motorSteps[1]; - - if(DEBUG_ODOMETRY)std::cout << "[" << epuckname << "] " << "left, right raw: " << motorSteps[0] << ", " << motorSteps[1] << std::endl; - if(DEBUG_ODOMETRY)std::cout << "[" << epuckname << "] " << "left, right raw corrected: " << motorPositionDataCorrect[0] << ", " << motorPositionDataCorrect[1] << std::endl; - - // Compute odometry. - leftStepsDiff = motorPositionDataCorrect[0]*MOT_STEP_DIST - leftStepsPrev; // Expressed in meters. - rightStepsDiff = motorPositionDataCorrect[1]*MOT_STEP_DIST - rightStepsPrev; // Expressed in meters. - if(DEBUG_ODOMETRY)std::cout << "[" << epuckname << "] " << "left, right steps diff: " << leftStepsDiff << ", " << rightStepsDiff << std::endl; - - deltaTheta = (rightStepsDiff - leftStepsDiff)/WHEEL_DISTANCE; // Expressed in radiant. - deltaSteps = (rightStepsDiff + leftStepsDiff)/2; // Expressed in meters. - if(DEBUG_ODOMETRY)std::cout << "[" << epuckname << "] " << "delta theta, steps: " << deltaTheta << ", " << deltaSteps << std::endl; - - xPos += deltaSteps*cos(theta + deltaTheta/2); // Expressed in meters. - yPos += deltaSteps*sin(theta + deltaTheta/2); // Expressed in meters. - theta += deltaTheta; // Expressed in radiant. - if(DEBUG_ODOMETRY)std::cout << "[" << epuckname << "] " << "x, y, theta: " << xPos << ", " << yPos << ", " << theta << std::endl; - - leftStepsPrev = motorPositionDataCorrect[0]*MOT_STEP_DIST; // Expressed in meters. - rightStepsPrev = motorPositionDataCorrect[1]*MOT_STEP_DIST; // Expressed in meters. - - // Publish the odometry message over ROS. - odomMsg.header.stamp = ros::Time::now(); - odomMsg.header.frame_id = "odom"; - ss << epuckname << "/base_link"; - odomMsg.child_frame_id = ss.str(); - odomMsg.pose.pose.position.x = xPos; - odomMsg.pose.pose.position.y = yPos; - odomMsg.pose.pose.position.z = 0; - odomMsg.pose.covariance[0] = 0.001; - odomMsg.pose.covariance[7] = 0.001; - odomMsg.pose.covariance[14] = 1000.0; - odomMsg.pose.covariance[21] = 1000.0; - odomMsg.pose.covariance[28] = 1000.0; - odomMsg.pose.covariance[35] = 0.001; - // Since all odometry is 6DOF we'll need a quaternion created from yaw. - orientQuat = tf::createQuaternionMsgFromYaw(theta); - odomMsg.pose.pose.orientation = orientQuat; - currentTime = ros::Time::now(); - odomMsg.twist.twist.linear.x = deltaSteps / ((currentTime-lastTime).toSec()); // "deltaSteps" is the linear distance covered in meters from the last update (delta distance); - // the time from the last update is measured in seconds thus to get m/s we multiply them. - odomMsg.twist.twist.angular.z = deltaTheta / ((currentTime-lastTime).toSec()); // "deltaTheta" is the angular distance covered in radiant from the last update (delta angle); - // the time from the last update is measured in seconds thus to get rad/s we multiply them. - if(DEBUG_ODOMETRY)std::cout << "[" << epuckname << "] " << "time elapsed = " << (currentTime-lastTime).toSec() << " seconds" << std::endl; - lastTime = ros::Time::now(); - - odomPublisher.publish(odomMsg); - - // Publish the transform over tf. - geometry_msgs::TransformStamped odomTrans; - odomTrans.header.stamp = odomMsg.header.stamp; - odomTrans.header.frame_id = odomMsg.header.frame_id; - odomTrans.child_frame_id = odomMsg.child_frame_id; - odomTrans.transform.translation.x = xPos; - odomTrans.transform.translation.y = yPos; - odomTrans.transform.translation.z = 0.0; - odomTrans.transform.rotation = orientQuat; - br.sendTransform(odomTrans); - //############################################################################################################################################# - - //############################################################################################################################################# - // IMU topic - ss.str(std::string()); - ss << epuckname << "/base_link"; - imuMsg.header.frame_id = ss.str(); - imuMsg.header.stamp = ros::Time::now(); - // Exchange x and y to be compatible with e-puck 1.x - imuMsg.linear_acceleration.x = (accData[1])/800.0*STANDARD_GRAVITY; // 1 g = about 800, then transforms in m/s^2. - imuMsg.linear_acceleration.y = (accData[0])/800.0*STANDARD_GRAVITY; - imuMsg.linear_acceleration.z = (accData[2])/800.0*STANDARD_GRAVITY; - imuMsg.linear_acceleration_covariance[0] = 0.01; - imuMsg.linear_acceleration_covariance[1] = 0.0; - imuMsg.linear_acceleration_covariance[2] = 0.0; - imuMsg.linear_acceleration_covariance[3] = 0.0; - imuMsg.linear_acceleration_covariance[4] = 0.01; - imuMsg.linear_acceleration_covariance[5] = 0.0; - imuMsg.linear_acceleration_covariance[6] = 0.0; - imuMsg.linear_acceleration_covariance[7] = 0.0; - imuMsg.linear_acceleration_covariance[8] = 0.01; - if(DEBUG_IMU)std::cout << "[" << epuckname << "] " << "accel raw: " << accData[0] << ", " << accData[1] << ", " << accData[2] << std::endl; - if(DEBUG_IMU)std::cout << "[" << epuckname << "] " << "accel (m/s2): " << imuMsg.linear_acceleration.x << ", " << imuMsg.linear_acceleration.y << ", " << imuMsg.linear_acceleration.z << std::endl; - imuMsg.angular_velocity.x = (gyroRaw[0] - gyroOffset[0]) * DEG2RAD(GYRO_RAW2DPS); // rad/s - imuMsg.angular_velocity.y = (gyroRaw[1] - gyroOffset[1]) * DEG2RAD(GYRO_RAW2DPS); - imuMsg.angular_velocity.z = (gyroRaw[2] - gyroOffset[2]) * DEG2RAD(GYRO_RAW2DPS); - imuMsg.angular_velocity_covariance[0] = 0.01; - imuMsg.angular_velocity_covariance[1] = 0.0; - imuMsg.angular_velocity_covariance[2] = 0.0; - imuMsg.angular_velocity_covariance[3] = 0.0; - imuMsg.angular_velocity_covariance[4] = 0.01; - imuMsg.angular_velocity_covariance[5] = 0.0; - imuMsg.angular_velocity_covariance[6] = 0.0; - imuMsg.angular_velocity_covariance[7] = 0.0; - imuMsg.angular_velocity_covariance[8] = 0.01; - if(DEBUG_IMU)std::cout << "[" << epuckname << "] " << "gyro raw: " << gyroRaw[0] << ", " << gyroRaw[1] << ", " << gyroRaw[2] << std::endl; - if(DEBUG_IMU)std::cout << "[" << epuckname << "] " << "gyro (rad/s): " << imuMsg.angular_velocity.x << ", " << imuMsg.angular_velocity.y << ", " << imuMsg.angular_velocity.z << std::endl; - // Pitch and roll computed assuming the aerospace rotation sequence Rxyz - double roll = atan2(imuMsg.linear_acceleration.y, imuMsg.linear_acceleration.z); - double pitch = atan2(-imuMsg.linear_acceleration.x, sqrt(imuMsg.linear_acceleration.y*imuMsg.linear_acceleration.y + imuMsg.linear_acceleration.z*imuMsg.linear_acceleration.z)); - double yaw = 0.0; - orientQuat = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); - imuMsg.orientation = orientQuat; - imuMsg.orientation_covariance[0] = 0.01; - imuMsg.orientation_covariance[1] = 0.0; - imuMsg.orientation_covariance[2] = 0.0; - imuMsg.orientation_covariance[3] = 0.0; - imuMsg.orientation_covariance[4] = 0.01; - imuMsg.orientation_covariance[5] = 0.0; - imuMsg.orientation_covariance[6] = 0.0; - imuMsg.orientation_covariance[7] = 0.0; - imuMsg.orientation_covariance[8] = 0.01; - if(DEBUG_IMU)std::cout << "[" << epuckname << "] " << "roll=" << roll << ", pitch=" << pitch << std::endl; - imuPublisher.publish(imuMsg); - //############################################################################################################################################# - - //############################################################################################################################################# - // Magnetic field topic - ss.str(std::string()); - ss << epuckname << "/base_link"; - magFieldMsg.header.frame_id = ss.str(); - magFieldMsg.header.stamp = ros::Time::now(); - magFieldMsg.magnetic_field_covariance[0] = 0.01; - magFieldMsg.magnetic_field_covariance[4] = 0.01; - magFieldMsg.magnetic_field_covariance[8] = 0.01; - magFieldMsg.magnetic_field.x = magneticField[0]/1000000.0; // given in Tesla - magFieldMsg.magnetic_field.y = magneticField[1]/1000000.0; // given in Tesla - magFieldMsg.magnetic_field.z = magneticField[2]/1000000.0; // given in Tesla - if(DEBUG_MAG_FIELD)std::cout << "[" << epuckname << "] " << "mag field (Tesla): " << magFieldMsg.magnetic_field.x << ", " << magFieldMsg.magnetic_field.y << ", " << magFieldMsg.magnetic_field.z << std::endl; - magFieldPublisher.publish(magFieldMsg); - // Magnetic field vector (normalized) topic - // The resulting vector will have a length of 1 meter - ss.str(std::string()); - ss << epuckname << "/base_link"; - magFieldVectorMsg.header.frame_id = ss.str(); - magFieldVectorMsg.header.stamp = ros::Time::now(); - magFieldVectorMsg.type = visualization_msgs::Marker::ARROW; - geometry_msgs::Point p; - // Start point - p.x = 0.0; - p.y = 0.0; - p.z = 0.0; - magFieldVectorMsg.points.clear(); - magFieldVectorMsg.points.push_back(p); - double mod = sqrt(magFieldMsg.magnetic_field.x*magFieldMsg.magnetic_field.x + magFieldMsg.magnetic_field.y*magFieldMsg.magnetic_field.y); - // End point - p.x = magFieldMsg.magnetic_field.x/mod; - p.y = magFieldMsg.magnetic_field.y/mod; - magFieldVectorMsg.points.push_back(p); - magFieldVectorMsg.scale.x = 0.002; - magFieldVectorMsg.scale.y = 0.003; - magFieldVectorMsg.scale.z = 0.005; - magFieldVectorMsg.color.a = 1.0; - magFieldVectorMsg.color.r = 1.0; - magFieldVectorMsg.color.g = 1.0; - magFieldVectorMsg.color.b = 0.0; - magFieldVectorPublisher.publish(magFieldVectorMsg); - //############################################################################################################################################# - - //############################################################################################################################################# - // Microphone topic - ss.str(std::string()); - ss << epuckname << "/base_link"; - microphoneMsg.header.frame_id = ss.str(); - microphoneMsg.header.stamp = ros::Time::now(); - microphoneMsg.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - microphoneMsg.pose.position.x = 0.15; - microphoneMsg.pose.position.y = 0; - microphoneMsg.pose.position.z = 0.11; - orientQuat = tf::createQuaternionMsgFromYaw(0); - microphoneMsg.pose.orientation = orientQuat; - microphoneMsg.scale.z = 0.01; - microphoneMsg.color.a = 1.0; - microphoneMsg.color.r = 1.0; - microphoneMsg.color.g = 1.0; - microphoneMsg.color.b = 1.0; - ss.str(std::string());; - ss << "mic: [" << micVolume[0] << ", " << micVolume[1] << ", " << micVolume[2] << ", " << micVolume[3] << "]"; - microphoneMsg.text = ss.str(); - microphonePublisher.publish(microphoneMsg); - //############################################################################################################################################# - - //############################################################################################################################################# - // Time of flight topic - distSensMsg.range = (float)distanceMm/1000.0; - if(distSensMsg.range > distSensMsg.max_range) { - distSensMsg.range = distSensMsg.max_range; - } - if(distSensMsg.range < distSensMsg.min_range) { - distSensMsg.range = distSensMsg.min_range; - } - distSensMsg.header.stamp = ros::Time::now(); - distSensPublisher.publish(distSensMsg); - transform.setOrigin( tf::Vector3(0.035, 0.0, 0.034) ); - q.setRPY(0, -0.21, 0.0); - transform.setRotation(q); - parent.str(""); - parent << epuckname << "/base_dist_sens"; - child.str(""); - child << epuckname << "/base_link"; - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), child.str(), parent.str())); - //############################################################################################################################################# - - //############################################################################################################################################# - // Battery topic - ss.str(std::string()); - ss << epuckname << "/base_link"; - battMsg.header.frame_id = ss.str(); - battMsg.header.stamp = ros::Time::now(); - battMsg.voltage = (float)batteryRaw / COEFF_ADC_TO_VOLT; - battMsg.percentage = (battMsg.voltage - MIN_VOLTAGE) / (MAX_VOLTAGE - MIN_VOLTAGE); - battMsg.present = 1; - if(DEBUG_BATTERY)std::cout << "[" << epuckname << "] " << "battery V: " << battMsg.voltage << ", " << battMsg.percentage*100.0 << " %" << std::endl; - battPublisher.publish(battMsg); - //############################################################################################################################################# - - //############################################################################################################################################# - // Camera image topic - if(camera_enabled) { - if(newImageReceived) { - newImageReceived = false; - cv::Mat rgb888; - cv_bridge::CvImage out_msg; - out_msg.header.stamp = ros::Time::now();; // Same timestamp and tf frame as input image - rgb888 = cv::Mat(120, 160, CV_8UC3); - RGB565toRGB888(160, 120, &image[0], rgb888.data); - out_msg.encoding = sensor_msgs::image_encodings::RGB8; - out_msg.image = rgb888; - imagePublisher.publish(out_msg.toImageMsg()); + void RGB565toRGB888(int width, int height, unsigned char *src, unsigned char *dst) + { + int line, column; + int index_src = 0, index_dst = 0; + + for (line = 0; line < height; ++line) + { + for (column = 0; column < width; ++column) + { + dst[index_dst++] = (unsigned char)(src[index_src] & 0xF8); + dst[index_dst++] = + (unsigned char)((src[index_src] & 0x07) << 5) | (unsigned char)((src[index_src + 1] & 0xE0) >> 3); + dst[index_dst++] = (unsigned char)((src[index_src + 1] & 0x1F) << 3); + index_src += 2; + } } } - //############################################################################################################################################# - - //############################################################################################################################################# - // Ground sensor topic - if(ground_sensors_enabled) { + + void PublishSensorStaticTransforms() + { + // Publish the static transforms for the sensors once, as they don't change relative to one another. + tf2::Transform transform; + tf2::Quaternion q; + std::stringstream parent; + std::stringstream child; + std::vector transforms; + transforms.reserve(10); + + auto MakeTfMsg = [](const tf2::Transform &transform, const rclcpp::Time &time, auto child, auto parent) { + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = time; + tf_msg.header.frame_id = std::move(parent); + tf_msg.child_frame_id = std::move(child); + tf_msg.transform = tf2::toMsg(transform); + return tf_msg; + }; + + // e-puck proximity positions (cm), x pointing forward, y pointing left + // P7(3.5, 1.0) P0(3.5, -1.0) + // P6(2.5, 2.5) P1(2.5, -2.5) + // P5(0.0, 3.0) P2(0.0, -3.0) + // P4(-3.5, 2.0) P3(-3.5, -2.0) + // + // e-puck proximity orentations (degrees) + // P7(10) P0(350) + // P6(40) P1(320) + // P5(90) P2(270) + // P4(160) P3(200) + + parent.str(""); + parent << epuckname << "/base_link"; + + transform.setOrigin(tf2::Vector3(0.035, -0.010, 0.034)); + q.setRPY(0, 0, 6.11); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_prox0"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + transform.setOrigin(tf2::Vector3(0.025, -0.025, 0.034)); + q.setRPY(0, 0, 5.59); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_prox1"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + transform.setOrigin(tf2::Vector3(0.000, -0.030, 0.034)); + q.setRPY(0, 0, 4.71); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_prox2"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + transform.setOrigin(tf2::Vector3(-0.035, -0.020, 0.034)); + q.setRPY(0, 0, 3.49); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_prox3"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + transform.setOrigin(tf2::Vector3(-0.035, 0.020, 0.034)); + q.setRPY(0, 0, 2.8); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_prox4"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + transform.setOrigin(tf2::Vector3(0.000, 0.030, 0.034)); + q.setRPY(0, 0, 1.57); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_prox5"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + transform.setOrigin(tf2::Vector3(0.025, 0.025, 0.034)); + q.setRPY(0, 0, 0.70); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_prox6"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + transform.setOrigin(tf2::Vector3(0.035, 0.010, 0.034)); + q.setRPY(0, 0, 0.17); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_prox7"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + // Laser + transform.setOrigin(tf2::Vector3(0.0, 0.0, 0.034)); + q.setRPY(0, 0, 0); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_laser"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + // ToF + transform.setOrigin(tf2::Vector3(0.035, 0.0, 0.034)); + q.setRPY(0, -0.21, 0.0); + transform.setRotation(q); + child.str(""); + child << epuckname << "/base_dist_sens"; + transforms.push_back(MakeTfMsg(transform, this->get_clock()->now(), child.str(), parent.str())); + + static_br->sendTransform(transforms); + } + + void updateRosInfo() + { + std::stringstream ss; + geometry_msgs::msg::Quaternion orientQuat; + std::stringstream parent; + std::stringstream child; + + tf2::Transform transform; + tf2::Quaternion q; + + int i = 0; + + // ############################################################################################################################################# + // Proximity topics + for (i = 0; i < 8; i++) + { + if (proxData[i] > 0) + { + proxMsg[i].range = 0.5 / sqrt(proxData[i]); // Transform the analog value to a distance value in meters + // (given from field tests). + } else + { + proxMsg[i].range = proxMsg[i].max_range; + } + if (proxMsg[i].range > proxMsg[i].max_range) { proxMsg[i].range = proxMsg[i].max_range; } + if (proxMsg[i].range < proxMsg[i].min_range) { proxMsg[i].range = proxMsg[i].min_range; } + proxMsg[i].header.stamp = this->get_clock()->now(); + proxPublisher[i]->publish(proxMsg[i]); + } + // ############################################################################################################################################# + + // ############################################################################################################################################# + // Laserscan topic + currentTimeMap = this->get_clock()->now(); + parent.str(""); + parent << epuckname << "/base_laser"; + // populate the LaserScan message + laserMsg.header.stamp = this->get_clock()->now(); + laserMsg.header.frame_id = parent.str(); + laserMsg.angle_min = -M_PI / 2.0; + laserMsg.angle_max = M_PI / 2.0; + laserMsg.angle_increment = M_PI / 18.0; // 10 degrees. + // laserMsg.time_increment = (currentTimeMap-lastTimeMap).toSec()/180; //0.003; //(1 / laser_frequency) / + // (num_readings); laserMsg.scan_time = (currentTimeMap-lastTimeMap).toSec(); + // The laser is placed in the center of the robot, but the proximity sensors are placed around the robot thus + // add "ROBOT_RADIUS" to get correct values. + laserMsg.range_min = 0.005 + ROBOT_RADIUS; // 0.5 cm + ROBOT_RADIUS. + laserMsg.range_max = 0.05 + ROBOT_RADIUS; // 5 cm + ROBOT_RADIUS. + laserMsg.ranges.resize(19); + laserMsg.intensities.resize(19); + lastTimeMap = this->get_clock()->now(); + + // We use the information from the 6 proximity sensors on the front side of the robot to get 19 laser scan + // points. The interpolation used is the following: -90 degrees: P2 -80 degrees: 4/5*P2 + 1/5*P1 -70 degrees: + // 3/5*P2 + 2/5*P1 -60 degrees: 2/5*P2 + 3/5*P1 -50 degrees: 1/5*P2 + 4/5*P1 -40 degrees: P1 -30 degrees: 2/3*P1 + // + 1/3*P0 -20 degrees: 1/3*P1 + 2/3*P0 -10 degrees: P0 0 degrees: 1/2*P0 + 1/2*P7 10 degrees: P7 20 degrees: + // 1/3*P6 + 2/3*P7 30 degrees: 2/3*P6 + 1/3*P7 40 degrees: P6 50 degrees: 1/5*P5 + 4/5*P6 60 degrees: 2/5*P5 + + // 3/5*P6 70 degrees: 3/5*P5 + 2/5*P6 80 degrees: 4/5*P5 + 1/5*P6 90 degrees: P5 + + float tempProx; + tempProx = proxData[2]; + if (tempProx > 0) + { + laserMsg.ranges[0] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[0] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[0] = laserMsg.range_max; + laserMsg.intensities[0] = 0; + } + + tempProx = proxData[2] * 4 / 5 + proxData[1] * 1 / 5; + if (tempProx > 0) + { + laserMsg.ranges[1] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[1] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[1] = laserMsg.range_max; + laserMsg.intensities[1] = 0; + } + + tempProx = proxData[2] * 3 / 5 + proxData[1] * 2 / 5; + if (tempProx > 0) + { + laserMsg.ranges[2] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[2] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[2] = laserMsg.range_max; + laserMsg.intensities[2] = 0; + } + + tempProx = proxData[2] * 2 / 5 + proxData[1] * 3 / 5; + if (tempProx > 0) + { + laserMsg.ranges[3] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[3] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[3] = laserMsg.range_max; + laserMsg.intensities[3] = 0; + } + + tempProx = proxData[2] * 1 / 5 + proxData[1] * 4 / 5; + if (tempProx > 0) + { + laserMsg.ranges[4] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[4] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[4] = laserMsg.range_max; + laserMsg.intensities[4] = 0; + } + + tempProx = proxData[1]; + if (tempProx > 0) + { + laserMsg.ranges[5] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[5] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[5] = laserMsg.range_max; + laserMsg.intensities[5] = 0; + } + + tempProx = proxData[1] * 2 / 3 + proxData[0] * 1 / 3; + if (tempProx > 0) + { + laserMsg.ranges[6] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[6] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[6] = laserMsg.range_max; + laserMsg.intensities[6] = 0; + } + + tempProx = proxData[1] * 1 / 3 + proxData[0] * 2 / 3; + if (tempProx > 0) + { + laserMsg.ranges[7] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[7] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[7] = laserMsg.range_max; + laserMsg.intensities[7] = 0; + } + + tempProx = proxData[0]; + if (tempProx > 0) + { + laserMsg.ranges[8] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[8] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[8] = laserMsg.range_max; + laserMsg.intensities[8] = 0; + } + + tempProx = (proxData[0] + proxData[7]) >> 1; + if (tempProx > 0) + { + laserMsg.ranges[9] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[9] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[9] = laserMsg.range_max; + laserMsg.intensities[9] = 0; + } + + tempProx = proxData[7]; + if (tempProx > 0) + { + laserMsg.ranges[10] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[10] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[10] = laserMsg.range_max; + laserMsg.intensities[10] = 0; + } + + tempProx = proxData[7] * 2 / 3 + proxData[6] * 1 / 3; + if (tempProx > 0) + { + laserMsg.ranges[11] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[11] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[11] = laserMsg.range_max; + laserMsg.intensities[11] = 0; + } + + tempProx = proxData[7] * 1 / 3 + proxData[6] * 2 / 3; + if (tempProx > 0) + { + laserMsg.ranges[12] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[12] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[12] = laserMsg.range_max; + laserMsg.intensities[12] = 0; + } + + tempProx = proxData[6]; + if (tempProx > 0) + { + laserMsg.ranges[13] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[13] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[13] = laserMsg.range_max; + laserMsg.intensities[13] = 0; + } + + tempProx = proxData[6] * 4 / 5 + proxData[5] * 1 / 5; + if (tempProx > 0) + { + laserMsg.ranges[14] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[14] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[14] = laserMsg.range_max; + laserMsg.intensities[14] = 0; + } + + tempProx = proxData[6] * 3 / 5 + proxData[5] * 2 / 5; + if (tempProx > 0) + { + laserMsg.ranges[15] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[15] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[15] = laserMsg.range_max; + laserMsg.intensities[15] = 0; + } + + tempProx = proxData[6] * 2 / 5 + proxData[5] * 3 / 5; + if (tempProx > 0) + { + laserMsg.ranges[16] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[16] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[16] = laserMsg.range_max; + laserMsg.intensities[16] = 0; + } + + tempProx = proxData[6] * 1 / 5 + proxData[5] * 4 / 5; + if (tempProx > 0) + { + laserMsg.ranges[17] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[17] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[17] = laserMsg.range_max; + laserMsg.intensities[17] = 0; + } + + tempProx = proxData[5]; + if (tempProx > 0) + { + laserMsg.ranges[18] = + (0.5 / sqrt(tempProx)) + + ROBOT_RADIUS; // Transform the analog value to a distance value in meters (given from field tests). + laserMsg.intensities[18] = tempProx; + } else + { // Sometimes the values could be negative due to the calibration, it means there is no obstacles. + laserMsg.ranges[18] = laserMsg.range_max; + laserMsg.intensities[18] = 0; + } + + for (i = 0; i < 19; i++) + { + if (laserMsg.ranges[i] > laserMsg.range_max) { laserMsg.ranges[i] = laserMsg.range_max; } + if (laserMsg.ranges[i] < laserMsg.range_min) { laserMsg.ranges[i] = laserMsg.range_min; } + } + + laserPublisher->publish(laserMsg); + // ############################################################################################################################################# + + // ############################################################################################################################################# + // Odometry topic + // The encoders values coming from the e-puck are 2 bytes signed int thus we need to handle the overflows + // otherwise the odometry will be wrong after a while (about 4 meters). + if ((leftStepsRawPrev > 0) && (motorSteps[0] < 0) && (abs(motorSteps[0] - leftStepsRawPrev) > 30000)) + { // Overflow detected (positive). + overflowCountLeft++; + } + if ((leftStepsRawPrev < 0) && (motorSteps[0] > 0) && (abs(motorSteps[0] - leftStepsRawPrev) > 30000)) + { // Overflow detected (negative). + overflowCountLeft--; + } + motorPositionDataCorrect[0] = (overflowCountLeft * 65536) + motorSteps[0]; + + if ((rightStepsRawPrev > 0) && (motorSteps[1] < 0) && (abs(motorSteps[1] - rightStepsRawPrev) > 30000)) + { // Overflow detected (positive). + overflowCountRight++; + } + if ((rightStepsRawPrev < 0) && (motorSteps[1] > 0) && (abs(motorSteps[1] - rightStepsRawPrev) > 30000)) + { // Overflow detected (negative). + overflowCountRight--; + } + motorPositionDataCorrect[1] = (overflowCountRight * 65536) + motorSteps[1]; + + leftStepsRawPrev = motorSteps[0]; + rightStepsRawPrev = motorSteps[1]; + + if (DEBUG_ODOMETRY) + std::cout << "[" << epuckname << "] " << "left, right raw: " << motorSteps[0] << ", " << motorSteps[1] + << std::endl; + if (DEBUG_ODOMETRY) + std::cout << "[" << epuckname << "] " << "left, right raw corrected: " << motorPositionDataCorrect[0] + << ", " << motorPositionDataCorrect[1] << std::endl; + + // Compute odometry. + leftStepsDiff = motorPositionDataCorrect[0] * MOT_STEP_DIST - leftStepsPrev; // Expressed in meters. + rightStepsDiff = motorPositionDataCorrect[1] * MOT_STEP_DIST - rightStepsPrev; // Expressed in meters. + if (DEBUG_ODOMETRY) + std::cout << "[" << epuckname << "] " << "left, right steps diff: " << leftStepsDiff << ", " + << rightStepsDiff << std::endl; + + deltaTheta = (rightStepsDiff - leftStepsDiff) / WHEEL_SEPARATION; // Expressed in radiant. + deltaSteps = (rightStepsDiff + leftStepsDiff) / 2; // Expressed in meters. + if (DEBUG_ODOMETRY) + std::cout << "[" << epuckname << "] " << "delta theta, steps: " << deltaTheta << ", " << deltaSteps + << std::endl; + + xPos += deltaSteps * cos(theta + deltaTheta / 2); // Expressed in meters. + yPos += deltaSteps * sin(theta + deltaTheta / 2); // Expressed in meters. + theta += deltaTheta; // Expressed in radiant. + if (DEBUG_ODOMETRY) + std::cout << "[" << epuckname << "] " << "x, y, theta: " << xPos << ", " << yPos << ", " << theta + << std::endl; + + leftStepsPrev = motorPositionDataCorrect[0] * MOT_STEP_DIST; // Expressed in meters. + rightStepsPrev = motorPositionDataCorrect[1] * MOT_STEP_DIST; // Expressed in meters. + + // Publish the odometry message over ROS. + odomMsg.header.stamp = this->get_clock()->now(); + ss.str(""); + ss << epuckname << "/odom"; + odomMsg.header.frame_id = ss.str(); + ss.str(""); + ss << epuckname << "/base_link"; + odomMsg.child_frame_id = ss.str(); + odomMsg.pose.pose.position.x = xPos; + odomMsg.pose.pose.position.y = yPos; + odomMsg.pose.pose.position.z = 0; + odomMsg.pose.covariance[0] = 0.001; + odomMsg.pose.covariance[7] = 0.001; + odomMsg.pose.covariance[14] = 1000.0; + odomMsg.pose.covariance[21] = 1000.0; + odomMsg.pose.covariance[28] = 1000.0; + odomMsg.pose.covariance[35] = 0.001; + // Since all odometry is 6DOF we'll need a quaternion created from yaw. + q.setRPY(0, 0, theta); + orientQuat = tf2::toMsg(q); + odomMsg.pose.pose.orientation = orientQuat; + currentTime = this->get_clock()->now(); + odomMsg.twist.twist.linear.x = + deltaSteps + / ((currentTime - lastTime).seconds()); // "deltaSteps" is the linear distance covered in meters from the + // last update (delta distance); the time from the last update is + // measured in seconds thus to get m/s we multiply them. + odomMsg.twist.twist.angular.z = + deltaTheta + / ((currentTime - lastTime).seconds()); // "deltaTheta" is the angular distance covered in radiant from the + // last update (delta angle); the time from the last update is + // measured in seconds thus to get rad/s we multiply them. + if (DEBUG_ODOMETRY) + std::cout << "[" << epuckname << "] " << "time elapsed = " << (currentTime - lastTime).seconds() + << " seconds" << std::endl; + lastTime = this->get_clock()->now(); + + odomPublisher->publish(odomMsg); + + // Publish the transform over tf. + geometry_msgs::msg::TransformStamped odomTrans; + odomTrans.header.stamp = odomMsg.header.stamp; + odomTrans.header.frame_id = odomMsg.header.frame_id; + odomTrans.child_frame_id = odomMsg.child_frame_id; + odomTrans.transform.translation.x = xPos; + odomTrans.transform.translation.y = yPos; + odomTrans.transform.translation.z = 0.0; + odomTrans.transform.rotation = orientQuat; + br->sendTransform(odomTrans); + odomLastPublished = odomMsg.header.stamp; + // ############################################################################################################################################# + + // ############################################################################################################################################# + // IMU topic + ss.str(std::string()); + ss << epuckname << "/base_link"; + imuMsg.header.frame_id = ss.str(); + imuMsg.header.stamp = this->get_clock()->now(); + // Exchange x and y to be compatible with e-puck 1.x + imuMsg.linear_acceleration.x = + (accData[1]) / 800.0 * STANDARD_GRAVITY; // 1 g = about 800, then transforms in m/s^2. + imuMsg.linear_acceleration.y = (accData[0]) / 800.0 * STANDARD_GRAVITY; + imuMsg.linear_acceleration.z = (accData[2]) / 800.0 * STANDARD_GRAVITY; + imuMsg.linear_acceleration_covariance[0] = 0.01; + imuMsg.linear_acceleration_covariance[1] = 0.0; + imuMsg.linear_acceleration_covariance[2] = 0.0; + imuMsg.linear_acceleration_covariance[3] = 0.0; + imuMsg.linear_acceleration_covariance[4] = 0.01; + imuMsg.linear_acceleration_covariance[5] = 0.0; + imuMsg.linear_acceleration_covariance[6] = 0.0; + imuMsg.linear_acceleration_covariance[7] = 0.0; + imuMsg.linear_acceleration_covariance[8] = 0.01; + if (DEBUG_IMU) + std::cout << "[" << epuckname << "] " << "accel raw: " << accData[0] << ", " << accData[1] << ", " + << accData[2] << std::endl; + if (DEBUG_IMU) + std::cout << "[" << epuckname << "] " << "accel (m/s2): " << imuMsg.linear_acceleration.x << ", " + << imuMsg.linear_acceleration.y << ", " << imuMsg.linear_acceleration.z << std::endl; + imuMsg.angular_velocity.x = (gyroRaw[0] - gyroOffset[0]) * DEG2RAD(GYRO_RAW2DPS); // rad/s + imuMsg.angular_velocity.y = (gyroRaw[1] - gyroOffset[1]) * DEG2RAD(GYRO_RAW2DPS); + imuMsg.angular_velocity.z = (gyroRaw[2] - gyroOffset[2]) * DEG2RAD(GYRO_RAW2DPS); + imuMsg.angular_velocity_covariance[0] = 0.01; + imuMsg.angular_velocity_covariance[1] = 0.0; + imuMsg.angular_velocity_covariance[2] = 0.0; + imuMsg.angular_velocity_covariance[3] = 0.0; + imuMsg.angular_velocity_covariance[4] = 0.01; + imuMsg.angular_velocity_covariance[5] = 0.0; + imuMsg.angular_velocity_covariance[6] = 0.0; + imuMsg.angular_velocity_covariance[7] = 0.0; + imuMsg.angular_velocity_covariance[8] = 0.01; + if (DEBUG_IMU) + std::cout << "[" << epuckname << "] " << "gyro raw: " << gyroRaw[0] << ", " << gyroRaw[1] << ", " + << gyroRaw[2] << std::endl; + if (DEBUG_IMU) + std::cout << "[" << epuckname << "] " << "gyro (rad/s): " << imuMsg.angular_velocity.x << ", " + << imuMsg.angular_velocity.y << ", " << imuMsg.angular_velocity.z << std::endl; + // Pitch and roll computed assuming the aerospace rotation sequence Rxyz + double roll = atan2(imuMsg.linear_acceleration.y, imuMsg.linear_acceleration.z); + double pitch = + atan2(-imuMsg.linear_acceleration.x, sqrt(imuMsg.linear_acceleration.y * imuMsg.linear_acceleration.y + + imuMsg.linear_acceleration.z * imuMsg.linear_acceleration.z)); + double yaw = 0.0; + q.setRPY(roll, pitch, yaw); + orientQuat = tf2::toMsg(q); + imuMsg.orientation = orientQuat; + imuMsg.orientation_covariance[0] = 0.01; + imuMsg.orientation_covariance[1] = 0.0; + imuMsg.orientation_covariance[2] = 0.0; + imuMsg.orientation_covariance[3] = 0.0; + imuMsg.orientation_covariance[4] = 0.01; + imuMsg.orientation_covariance[5] = 0.0; + imuMsg.orientation_covariance[6] = 0.0; + imuMsg.orientation_covariance[7] = 0.0; + imuMsg.orientation_covariance[8] = 0.01; + if (DEBUG_IMU) std::cout << "[" << epuckname << "] " << "roll=" << roll << ", pitch=" << pitch << std::endl; + imuPublisher->publish(imuMsg); + // ############################################################################################################################################# + + // ############################################################################################################################################# + // Magnetic field topic ss.str(std::string()); ss << epuckname << "/base_link"; - floorMsg.header.frame_id = ss.str(); - floorMsg.header.stamp = ros::Time::now(); - floorMsg.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - floorMsg.pose.position.x = 0.15; - floorMsg.pose.position.y = 0; - floorMsg.pose.position.z = 0.13; - orientQuat = tf::createQuaternionMsgFromYaw(0); - floorMsg.pose.orientation = orientQuat; - floorMsg.scale.z = 0.01; - floorMsg.color.a = 1.0; - floorMsg.color.r = 1.0; - floorMsg.color.g = 1.0; - floorMsg.color.b = 1.0; + magFieldMsg.header.frame_id = ss.str(); + magFieldMsg.header.stamp = this->get_clock()->now(); + magFieldMsg.magnetic_field_covariance[0] = 0.01; + magFieldMsg.magnetic_field_covariance[4] = 0.01; + magFieldMsg.magnetic_field_covariance[8] = 0.01; + magFieldMsg.magnetic_field.x = magneticField[0] / 1000000.0; // given in Tesla + magFieldMsg.magnetic_field.y = magneticField[1] / 1000000.0; // given in Tesla + magFieldMsg.magnetic_field.z = magneticField[2] / 1000000.0; // given in Tesla + if (DEBUG_MAG_FIELD) + std::cout << "[" << epuckname << "] " << "mag field (Tesla): " << magFieldMsg.magnetic_field.x << ", " + << magFieldMsg.magnetic_field.y << ", " << magFieldMsg.magnetic_field.z << std::endl; + magFieldPublisher->publish(magFieldMsg); + // Magnetic field vector (normalized) topic + // The resulting vector will have a length of 1 meter ss.str(std::string()); - ss << "floor: [" << groundProx[0] << ", " << groundProx[1] << ", " << groundProx[2] << ", " << groundProx[3] << ", " << groundProx[4] << "]"; - floorMsg.text = ss.str(); - floorPublisher.publish(floorMsg); + ss << epuckname << "/base_link"; + magFieldVectorMsg.header.frame_id = ss.str(); + magFieldVectorMsg.header.stamp = this->get_clock()->now(); + magFieldVectorMsg.type = visualization_msgs::msg::Marker::ARROW; + geometry_msgs::msg::Point p; + // Start point + p.x = 0.0; + p.y = 0.0; + p.z = 0.0; + magFieldVectorMsg.points.clear(); + magFieldVectorMsg.points.push_back(p); + double mod = sqrt(magFieldMsg.magnetic_field.x * magFieldMsg.magnetic_field.x + + magFieldMsg.magnetic_field.y * magFieldMsg.magnetic_field.y); + // End point + p.x = magFieldMsg.magnetic_field.x / mod; + p.y = magFieldMsg.magnetic_field.y / mod; + magFieldVectorMsg.points.push_back(p); + magFieldVectorMsg.scale.x = 0.002; + magFieldVectorMsg.scale.y = 0.003; + magFieldVectorMsg.scale.z = 0.005; + magFieldVectorMsg.color.a = 1.0; + magFieldVectorMsg.color.r = 1.0; + magFieldVectorMsg.color.g = 1.0; + magFieldVectorMsg.color.b = 0.0; + magFieldVectorPublisher->publish(magFieldVectorMsg); + // ############################################################################################################################################# + + // ############################################################################################################################################# + // Microphone topic + ss.str(std::string()); + ss << epuckname << "/base_link"; + microphoneMsg.header.frame_id = ss.str(); + microphoneMsg.header.stamp = this->get_clock()->now(); + microphoneMsg.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + microphoneMsg.pose.position.x = 0.15; + microphoneMsg.pose.position.y = 0; + microphoneMsg.pose.position.z = 0.11; + q.setRPY(0, 0, 0); + orientQuat = tf2::toMsg(q); + microphoneMsg.pose.orientation = orientQuat; + microphoneMsg.scale.z = 0.01; + microphoneMsg.color.a = 1.0; + microphoneMsg.color.r = 1.0; + microphoneMsg.color.g = 1.0; + microphoneMsg.color.b = 1.0; + ss.str(std::string()); + ; + ss << "mic: [" << micVolume[0] << ", " << micVolume[1] << ", " << micVolume[2] << ", " << micVolume[3] << "]"; + microphoneMsg.text = ss.str(); + microphonePublisher->publish(microphoneMsg); + // ############################################################################################################################################# + + // ############################################################################################################################################# + // Time of flight topic + distSensMsg.range = (float)distanceMm / 1000.0; + if (distSensMsg.range > distSensMsg.max_range) { distSensMsg.range = distSensMsg.max_range; } + if (distSensMsg.range < distSensMsg.min_range) { distSensMsg.range = distSensMsg.min_range; } + distSensMsg.header.stamp = this->get_clock()->now(); + distSensPublisher->publish(distSensMsg); + // ############################################################################################################################################# + + // ############################################################################################################################################# + // Battery topic + ss.str(std::string()); + ss << epuckname << "/base_link"; + battMsg.header.frame_id = ss.str(); + battMsg.header.stamp = this->get_clock()->now(); + battMsg.voltage = (float)batteryRaw / COEFF_ADC_TO_VOLT; + battMsg.percentage = (battMsg.voltage - MIN_VOLTAGE) / (MAX_VOLTAGE - MIN_VOLTAGE); + battMsg.present = 1; + if (DEBUG_BATTERY) + std::cout << "[" << epuckname << "] " << "battery V: " << battMsg.voltage << ", " + << battMsg.percentage * 100.0 << " %" << std::endl; + battPublisher->publish(battMsg); + // ############################################################################################################################################# + + // ############################################################################################################################################# + // Camera image topic + if (camera_enabled) + { + if (newImageReceived) + { + newImageReceived = false; + cv::Mat rgb888; + cv_bridge::CvImage out_msg; + out_msg.header.stamp = this->get_clock()->now(); + ; // Same timestamp and tf frame as input image + rgb888 = cv::Mat(120, 160, CV_8UC3); + RGB565toRGB888(160, 120, &image[0], rgb888.data); + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = rgb888; + auto msg = out_msg.toImageMsg(); + imagePublisher.publish(msg); + } + } + // ############################################################################################################################################# + + // ############################################################################################################################################# + // Ground sensor topic + if (ground_sensors_enabled) + { + ss.str(std::string()); + ss << epuckname << "/base_link"; + floorMsg.header.frame_id = ss.str(); + floorMsg.header.stamp = this->get_clock()->now(); + floorMsg.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + floorMsg.pose.position.x = 0.15; + floorMsg.pose.position.y = 0; + floorMsg.pose.position.z = 0.13; + q.setRPY(0, 0, 0); + orientQuat = tf2::toMsg(q); + floorMsg.pose.orientation = orientQuat; + floorMsg.scale.z = 0.01; + floorMsg.color.a = 1.0; + floorMsg.color.r = 1.0; + floorMsg.color.g = 1.0; + floorMsg.color.b = 1.0; + ss.str(std::string()); + ss << "floor: [" << groundProx[0] << ", " << groundProx[1] << ", " << groundProx[2] << ", " << groundProx[3] + << ", " << groundProx[4] << "]"; + floorMsg.text = ss.str(); + floorPublisher->publish(floorMsg); + } + // ############################################################################################################################################# } - //############################################################################################################################################# - -} -void handlerVelocity(const geometry_msgs::Twist::ConstPtr& msg) { - // Controls the velocity of each wheel based on linear and angular velocities. - double linear = msg->linear.x/3; // Divide by 3 to adapt the values received from the rviz "teleop" module that are too high. - double angular = msg->angular.z/3; - - // Kinematic model for differential robot. - double wl = (linear - (WHEEL_SEPARATION / 2.0) * angular) / WHEEL_DIAMETER; - double wr = (linear + (WHEEL_SEPARATION / 2.0) * angular) / WHEEL_DIAMETER; - - // At input 1000, angular velocity is 1 cycle / s or 2*pi/s. - speedLeft = int(wl * 1000.0); - speedRight = int(wr * 1000.0); - - command[3] = speedLeft & 0xFF; // left motor LSB - command[4] = speedLeft >> 8; // left motor MSB - command[5] = speedRight & 0xFF; // right motor LSB - command[6] = speedRight >> 8; // right motor MSB - - if(DEBUG_SPEED_RECEIVED)std::cout << "[" << epuckname << "] " << "new speed: " << speedLeft << ", " << speedRight << std::endl; - -} + void handlerVelocity(const geometry_msgs::msg::Twist::ConstSharedPtr &msg) + { + // Controls the velocity of each wheel based on linear and angular velocities. + double linear = msg->linear.x; + double angular = msg->angular.z; -void handlerLED(const std_msgs::UInt8MultiArray::ConstPtr& msg) { - // Controls the state of each LED on the standard robot - for (int i = 0; i < LED_NUMBER; i++) { - if(msg->data[i] == 0) { - command[7] &= ~(1<data[i] << ", "; - } -} + // Kinematic model for differential robot. + double wl = (linear - (WHEEL_SEPARATION / 2.0) * angular) / WHEEL_DIAMETER; + double wr = (linear + (WHEEL_SEPARATION / 2.0) * angular) / WHEEL_DIAMETER; -void handlerRgbLeds(const std_msgs::UInt8MultiArray::ConstPtr& msg) { - command[8] = msg->data[0]; // LED2 red - command[9] = msg->data[1]; // LED2 green - command[10] = msg->data[2]; // LED2 blue - command[11] = msg->data[3]; // LED4 red - command[12] = msg->data[4]; // LED4 green - command[13] = msg->data[5]; // LED4 blue - command[14] = msg->data[6]; // LED6 red - command[15] = msg->data[7]; // LED6 green - command[16] = msg->data[8]; // LED6 blue - command[17] = msg->data[9]; // LED8 red - command[18] = msg->data[10]; // LED8 green - command[19] = msg->data[11]; // LED8 blue - - if(DEBUG_RGB_RECEIVED) { - std::cout << "[" << epuckname << "] " << "new RGB status: " << std::endl; - for (int i = 0; i < RGB_LED_NUMBER; i++) { - std::cout << i << ": " << msg->data[i*3] << ", " << msg->data[i*3+1] << ", " << msg->data[i*3+2]; - } + // At input 1000, angular velocity is 1 cycle / s or 2*pi/s. + speedLeft = int(wl * 1000.0); + speedRight = int(wr * 1000.0); + + command[3] = speedLeft & 0xFF; // left motor LSB + command[4] = speedLeft >> 8; // left motor MSB + command[5] = speedRight & 0xFF; // right motor LSB + command[6] = speedRight >> 8; // right motor MSB + + if (DEBUG_SPEED_RECEIVED) + std::cout << "[" << epuckname << "] " << "new speed: " << speedLeft << ", " << speedRight << std::endl; } -} -int main(int argc,char *argv[]) { - int robotId = 0; - double init_xpos, init_ypos, init_theta; - int rosRate = 0; - unsigned int bufIndex = 0; - int i = 0; - - command[0] = 0x80; - command[1] = 2; // Sensors enabled. - command[2] = 1; // Calibrate proximity sensors. - command[3] = 0; // left motor LSB - command[4] = 0; // left motor MSB - command[5] = 0; // right motor LSB - command[6] = 0; // right motor MSB - command[7] = 0; // lEDs - command[8] = 0; // LED2 red - command[9] = 0; // LED2 green - command[10] = 0; // LED2 blue - command[11] = 0; // LED4 red - command[12] = 0; // LED4 green - command[13] = 0; // LED4 blue - command[14] = 0; // LED6 red - command[15] = 0; // LED6 green - command[16] = 0; // LED6 blue - command[17] = 0; // LED8 red - command[18] = 0; // LED8 green - command[19] = 0; // LED8 blue - command[20] = 0; // speaker - expected_recv_packets = 1; - - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. - * For programmatic remappings you can use a different version of init() which takes - * remappings directly, but for most command-line programs, passing argc and argv is - * the easiest way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of ros::init() before using any other - * part of the ROS system. - */ - ros::init(argc, argv, "epuck_driver_cpp"); + void handlerLED(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr &msg) + { + // Controls the state of each LED on the standard robot + for (int i = 0; i < LED_NUMBER; i++) + { + if (msg->data[i] == 0) + { + command[7] &= ~(1 << i); + } else + { + command[7] |= (1 << i); + } + } - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - ros::NodeHandle np("~"); // Private. - ros::NodeHandle n; // Public. - - np.getParam("epuck2_id", robotId); - np.param("epuck2_address", epuckAddress, ""); - np.param("epuck2_name", epuckname, "epuck2"); - np.param("xpos", init_xpos, 0.0); - np.param("ypos", init_ypos, 0.0); - np.param("theta", init_theta, 0.0); - np.param("camera", camera_enabled, false); - np.param("floor", ground_sensors_enabled, false); - //np.param("ros_rate", rosRate, 7); - - if(DEBUG_ROS_PARAMS) { - std::cout << "[" << epuckname << "] " << "epuck id: " << robotId << std::endl; - std::cout << "[" << epuckname << "] " << "epuck address: " << epuckAddress << std::endl; - std::cout << "[" << epuckname << "] " << "epuck name: " << epuckname << std::endl; - std::cout << "[" << epuckname << "] " << "init pose: " << init_xpos << ", " << init_ypos << ", " << theta << std::endl; - std::cout << "[" << epuckname << "] " << "camera enabled: " << camera_enabled << std::endl; - std::cout << "[" << epuckname << "] " << "ground sensors enabled: " << ground_sensors_enabled << std::endl; - //std::cout << "[" << epuckname << "] " << "ros rate: " << rosRate << std::endl; + if (DEBUG_LED_RECEIVED) + { + std::cout << "[" << epuckname << "] " << "new LED status: " << std::endl; + for (int i = 0; i < LED_NUMBER; i++) + std::cout << msg->data[i] << ", "; + } } - - if(epuckAddress.compare("")==0) { - std::cerr << "Robot address cannot be empty" << std::endl; - return -1; - } - if(initConnectionWithRobot()<0) { - std::cerr << "Can't connect to the robot" << std::endl; - exit(1); - } - - imuPublisher = n.advertise("imu", 10); - - for(i=0; i<8; i++) { - /** - * The advertise() function is how you tell ROS that you want to - * publish on a given topic name. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. After this advertise() call is made, the master - * node will notify anyone who is trying to subscribe to this topic name, - * and they will in turn negotiate a peer-to-peer connection with this - * node. advertise() returns a Publisher object which allows you to - * publish messages on that topic through a call to publish(). Once - * all copies of the returned Publisher object are destroyed, the topic - * will be automatically unadvertised. - * - * The second parameter to advertise() is the size of the message queue - * used for publishing messages. If messages are published more quickly - * than we can send them, the number here specifies how many messages to - * buffer up before throwing some away. - */ - std::stringstream ss; - ss.str(""); - ss << "proximity" << i; - proxPublisher[i] = n.advertise(ss.str(), 10); - //proxMsg[i] = new sensor_msgs::Range(); - proxMsg[i].radiation_type = sensor_msgs::Range::INFRARED; - ss.str(""); - ss << epuckname << "/base_prox" << i; - proxMsg[i].header.frame_id = ss.str(); - proxMsg[i].field_of_view = 0.26; // About 15 degrees...to be checked! - proxMsg[i].min_range = 0.005; // 0.5 cm. - proxMsg[i].max_range = 0.05; // 5 cm. - } - laserPublisher = n.advertise("scan", 10); - - odomPublisher = n.advertise("odom", 10); - currentTime = ros::Time::now(); - lastTime = ros::Time::now(); - - microphonePublisher = n.advertise("microphone", 10); - - distSensPublisher = n.advertise("dist_sens", 10); - distSensMsg.radiation_type = sensor_msgs::Range::INFRARED; - std::stringstream ss; - ss.str(""); - ss << epuckname << "/base_dist_sens"; - distSensMsg.header.frame_id = ss.str(); - distSensMsg.field_of_view = 0.43; // About 25 degrees (+/- 12.5) - distSensMsg.min_range = 0.005; // 5 mm. - distSensMsg.max_range = 2; // 2 m. - - magFieldPublisher = n.advertise("mag_field", 10); - magFieldVectorPublisher = n.advertise("mag_field_vector", 10); - - battPublisher = n.advertise("battery", 10); - - if(camera_enabled) { - imagePublisher = n.advertise("camera", 1); - command[1] = 3; // Camera and sensors enabled. - expected_recv_packets = 2; - } - - if(ground_sensors_enabled) { - floorPublisher = n.advertise("floor", 10); - } - - - /** - * The subscribe() call is how you tell ROS that you want to receive messages - * on a given topic. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. Messages are passed to a callback function, here - * called handlerVelocity. subscribe() returns a Subscriber object that you - * must hold on to until you want to unsubscribe. When all copies of the Subscriber - * object go out of scope, this callback will automatically be unsubscribed from - * this topic. - * - * The second parameter to the subscribe() function is the size of the message - * queue. If messages are arriving faster than they are being processed, this - * is the number of messages that will be buffered up before beginning to throw - * away the oldest ones. - */ - cmdVelSubscriber = n.subscribe("mobile_base/cmd_vel", 10, handlerVelocity); - cmdLedSubscriber = n.subscribe("mobile_base/cmd_led", 10, handlerLED); - cmdRgbLedsSubscriber = n.subscribe("mobile_base/rgb_leds", 10, handlerRgbLeds); - - theta = init_theta; - xPos = init_xpos; - yPos = init_ypos; - - //ros::Rate loop_rate(rosRate); - - while (ros::ok()) { - updateSensorsAndActuators(); - updateRosInfo(); - ros::spinOnce(); - //loop_rate.sleep(); // Do not call "sleep" otherwise the bluetooth communication will hang. - // We communicate as fast as possible, this shouldn't be a problem... + + void handlerRgbLeds(const std_msgs::msg::UInt8MultiArray::ConstSharedPtr &msg) + { + command[8] = msg->data[0]; // LED2 red + command[9] = msg->data[1]; // LED2 green + command[10] = msg->data[2]; // LED2 blue + command[11] = msg->data[3]; // LED4 red + command[12] = msg->data[4]; // LED4 green + command[13] = msg->data[5]; // LED4 blue + command[14] = msg->data[6]; // LED6 red + command[15] = msg->data[7]; // LED6 green + command[16] = msg->data[8]; // LED6 blue + command[17] = msg->data[9]; // LED8 red + command[18] = msg->data[10]; // LED8 green + command[19] = msg->data[11]; // LED8 blue + + if (DEBUG_RGB_RECEIVED) + { + std::cout << "[" << epuckname << "] " << "new RGB status: " << std::endl; + for (int i = 0; i < RGB_LED_NUMBER; i++) + { + std::cout << i << ": " << msg->data[i * 3] << ", " << msg->data[i * 3 + 1] << ", " + << msg->data[i * 3 + 2]; + } + } } - closeConnection(); + bool init() + { + int robotId = 0; + double init_xpos, init_ypos, init_theta; + int rosRate = 0; + unsigned int bufIndex = 0; + int i = 0; + + command[0] = 0x80; + command[1] = 2; // Sensors enabled. + command[2] = 1; // Calibrate proximity sensors. + command[3] = 0; // left motor LSB + command[4] = 0; // left motor MSB + command[5] = 0; // right motor LSB + command[6] = 0; // right motor MSB + command[7] = 0; // lEDs + command[8] = 0; // LED2 red + command[9] = 0; // LED2 green + command[10] = 0; // LED2 blue + command[11] = 0; // LED4 red + command[12] = 0; // LED4 green + command[13] = 0; // LED4 blue + command[14] = 0; // LED6 red + command[15] = 0; // LED6 green + command[16] = 0; // LED6 blue + command[17] = 0; // LED8 red + command[18] = 0; // LED8 green + command[19] = 0; // LED8 blue + command[20] = 0; // speaker + expected_recv_packets = 1; + + this->get_parameter("epuck2_id", robotId); + this->get_parameter_or("epuck2_address", epuckAddress, ""); + this->get_parameter_or("epuck2_port", epuckPort, 1000); + this->get_parameter_or("epuck2_name", epuckname, "epuck2"); + this->get_parameter("robot/xpos", init_xpos); + this->get_parameter("robot/ypos", init_ypos); + this->get_parameter("robot/theta", init_theta); + this->get_parameter("robot/camera", camera_enabled); + this->get_parameter("robot/floor", ground_sensors_enabled); + + // this->get_parameter_or("ros_rate", rosRate, 7); + + if (DEBUG_ROS_PARAMS) + { + std::cout << "[" << epuckname << "] " << "epuck id: " << robotId << std::endl; + std::cout << "[" << epuckname << "] " << "epuck address: " << epuckAddress << std::endl; + std::cout << "[" << epuckname << "] " << "epuck port: " << epuckPort << std::endl; + std::cout << "[" << epuckname << "] " << "epuck name: " << epuckname << std::endl; + std::cout << "[" << epuckname << "] " << "init pose: " << init_xpos << ", " << init_ypos << ", " << theta + << std::endl; + std::cout << "[" << epuckname << "] " << "camera enabled: " << camera_enabled << std::endl; + std::cout << "[" << epuckname << "] " << "ground sensors enabled: " << ground_sensors_enabled << std::endl; + // std::cout << "[" << epuckname << "] " << "ros rate: " << rosRate << std::endl; + } - -} + if (epuckAddress.compare("") == 0) + { + std::cerr << "Robot address cannot be empty" << std::endl; + return false; + } + if (initConnectionWithRobot() < 0) + { + std::cerr << "Can't connect to the robot" << std::endl; + return false; + } + + imuPublisher = this->create_publisher("imu", 10); + + for (i = 0; i < 8; i++) + { + /** + * The advertise() function is how you tell ROS that you want to + * publish on a given topic name. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. After this advertise() call is made, the master + * node will notify anyone who is trying to subscribe to this topic name, + * and they will in turn negotiate a peer-to-peer connection with this + * node. advertise() returns a Publisher object which allows you to + * publish messages on that topic through a call to publish(). Once + * all copies of the returned Publisher object are destroyed, the topic + * will be automatically unadvertised. + * + * The second parameter to advertise() is the size of the message queue + * used for publishing messages. If messages are published more quickly + * than we can send them, the number here specifies how many messages to + * buffer up before throwing some away. + */ + std::stringstream ss; + ss.str(""); + ss << "proximity" << i; + proxPublisher[i] = this->create_publisher(ss.str(), 10); + // proxMsg[i] = new sensor_msgs::Range(); + proxMsg[i].radiation_type = sensor_msgs::msg::Range::INFRARED; + ss.str(""); + ss << epuckname << "/base_prox" << i; + proxMsg[i].header.frame_id = ss.str(); + proxMsg[i].field_of_view = 0.26; // About 15 degrees...to be checked! + proxMsg[i].min_range = 0.005; // 0.5 cm. + proxMsg[i].max_range = 0.05; // 5 cm. + } + laserPublisher = this->create_publisher("scan", 10); + + odomPublisher = this->create_publisher("odom", 10); + currentTime = this->get_clock()->now(); + lastTime = this->get_clock()->now(); + + microphonePublisher = this->create_publisher("microphone", 10); + + distSensPublisher = this->create_publisher("dist_sens", 10); + distSensMsg.radiation_type = sensor_msgs::msg::Range::INFRARED; + std::stringstream ss; + ss.str(""); + ss << epuckname << "/base_dist_sens"; + distSensMsg.header.frame_id = ss.str(); + distSensMsg.field_of_view = 0.43; // About 25 degrees (+/- 12.5) + distSensMsg.min_range = 0.005; // 5 mm. + distSensMsg.max_range = 2; // 2 m. + + magFieldPublisher = this->create_publisher("mag_field", 10); + magFieldVectorPublisher = this->create_publisher("mag_field_vector", 10); + + battPublisher = this->create_publisher("battery", 10); + + if (camera_enabled) + { + imagePublisher = image_transport::create_publisher(this, "camera", {RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1}); + command[1] = 3; // Camera and sensors enabled. + expected_recv_packets = 2; + } + + if (ground_sensors_enabled) + { + floorPublisher = this->create_publisher("floor", 10); + } + /** + * The create_subscription() call is how you tell ROS that you want to receive messages + * on a given topic. Messages are passed to a callback function, here + * called handlerVelocity. create_subscription() returns a Subscription pointer that you + * must hold on to until you want to unsubscribe. + * + * The second parameter to the create_subscription() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ + cmdVelSubscriber = this->create_subscription( + "mobile_base/cmd_vel", 10, + [&](const geometry_msgs::msg::Twist::ConstSharedPtr &msg) { handlerVelocity(msg); }); + cmdLedSubscriber = this->create_subscription( + "mobile_base/cmd_led", 10, + [&](const std_msgs::msg::UInt8MultiArray::ConstSharedPtr &msg) { handlerLED(msg); }); + cmdRgbLedsSubscriber = this->create_subscription( + "mobile_base/rgb_leds", 10, + [&](const std_msgs::msg::UInt8MultiArray::ConstSharedPtr &msg) { handlerRgbLeds(msg); }); + + theta = init_theta; + xPos = init_xpos; + yPos = init_ypos; + + PublishSensorStaticTransforms(); + + using namespace std::chrono_literals; + tf_timer = this->create_timer(0.5s, [&](const rclcpp::TimerInfo &info) { + // Only publish the odom transform if the last one was published more than 0.45s ago. + auto x = info.actual_call_time - odomLastPublished; + if (x < 0.45s) { return; } + + // PublishSensorStaticTransforms(); + + // Publish the initial odom transform over tf. + geometry_msgs::msg::TransformStamped odomTrans; + odomTrans.header.stamp = this->get_clock()->now(); + ss.str(""); + ss << epuckname << "/odom"; + odomTrans.header.frame_id = ss.str(); + ss.str(""); + ss << epuckname << "/base_link"; + odomTrans.child_frame_id = ss.str(); + odomTrans.transform.translation.x = xPos; + odomTrans.transform.translation.y = yPos; + odomTrans.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, theta); + odomTrans.transform.rotation = tf2::toMsg(q); + br->sendTransform(odomTrans); + }); + + return true; + } +}; +int main(int argc, char *argv[]) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. + * For programmatic remappings you can use a different version of init() which takes + * remappings directly, but for most command-line programs, passing argc and argv is + * the easiest way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + rclcpp::init(argc, argv); + + // ros::Rate loop_rate(rosRate); + using namespace std::chrono_literals; + rclcpp::Rate loop_rate(0.1s); + + auto node = std::make_shared(); + if (!node->init()) { exit(1); } + + while (rclcpp::ok()) + { + node->updateSensorsAndActuators(); + node->updateRosInfo(); + rclcpp::spin_some(node); + // loop_rate.sleep(); + } + node->closeConnection(); +} diff --git a/urdf/epuck_urdf.xml b/urdf/epuck_urdf.xml index 59b8b55..7cc98e5 100644 --- a/urdf/epuck_urdf.xml +++ b/urdf/epuck_urdf.xml @@ -1,70 +1,423 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file