diff --git a/.gitmodules b/.gitmodules
index 32ff019b..139613cf 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,21 +1,21 @@
-[submodule "ros/opentera-webrtc-ros"]
- path = ros/opentera-webrtc-ros
- url = https://github.com/introlab/opentera-webrtc-ros.git
+[submodule "system/optional"]
+ path = system/optional
+ url = https://github.com/TartanLlama/optional.git
+[submodule "ros/utils/hbba_lite"]
+ path = ros/utils/hbba_lite
+ url = https://github.com/introlab/hbba_lite.git
+[submodule "ros/utils/audio_utils"]
+ path = ros/utils/audio_utils
+ url = https://github.com/introlab/audio_utils.git
[submodule "ros/perceptions/odas_ros"]
path = ros/perceptions/odas_ros
url = https://github.com/introlab/odas_ros.git
-[submodule "ros/utils/recorders/3rd_party/SQLiteCpp"]
- path = ros/utils/recorders/3rd_party/SQLiteCpp
- url = https://github.com/SRombauts/SQLiteCpp.git
[submodule "ros/demos/home_logger/3rd_party/fmt"]
path = ros/demos/home_logger/3rd_party/fmt
url = https://github.com/fmtlib/fmt.git
-[submodule "ros/utils/audio_utils"]
- path = ros/utils/audio_utils
- url = https://github.com/introlab/audio_utils.git
-[submodule "ros/utils/hbba_lite"]
- path = ros/utils/hbba_lite
- url = https://github.com/introlab/hbba_lite.git
-[submodule "system/optional"]
- path = system/optional
- url = https://github.com/TartanLlama/optional.git
+[submodule "ros/utils/recorders/3rd_party/SQLiteCpp"]
+ path = ros/utils/recorders/3rd_party/SQLiteCpp
+ url = https://github.com/SRombauts/SQLiteCpp.git
+[submodule "ros/utils/opentera-webrtc-ros"]
+ path = ros/utils/opentera-webrtc-ros
+ url = https://github.com/introlab/opentera-webrtc-ros.git
diff --git a/documentation/assembly/01_COMPUTER_CONFIGURATION.md b/documentation/assembly/01_COMPUTER_CONFIGURATION.md
index b9dd8f02..b2786f2e 100644
--- a/documentation/assembly/01_COMPUTER_CONFIGURATION.md
+++ b/documentation/assembly/01_COMPUTER_CONFIGURATION.md
@@ -712,3 +712,12 @@ export GOOGLE_APPLICATION_CREDENTIALS="[Path to the service account JSON keyfile
```bash
export OPEN_WEATHER_MAP_API_KEY="[The key]"
```
+
+### C. Setup a Different ROS2 Domain on Each Robot
+
+1. Add the following line to `~/.bashrc`
+
+```bash
+# Avoid default=0 and make sure each robot has a different domain ID.
+export ROS_DOMAIN_ID=[1...101]
+```
diff --git a/documentation/assembly/13_CALIBRATION.md b/documentation/assembly/13_CALIBRATION.md
index da6b359b..9f713475 100644
--- a/documentation/assembly/13_CALIBRATION.md
+++ b/documentation/assembly/13_CALIBRATION.md
@@ -4,11 +4,13 @@
1. Execute the following command.
```bash
-roslaunch t_top platform.launch camera_2d_wide_enabled:=true
+ros2 launch t_top platform.launch.xml camera_2d_wide_enabled:=true
```
2. Follow the following [steps](http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration).
```bash
-rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.69 image:=/camera_2d_wide_full_hd/image camera:=/camera_2d_wide_full_hd
+# Create calibration path
+mkdir -p ~/.ros/t-top/calibration
+ros2 run camera_calibration cameracalibrator --size 9x6 --square 0.051 --ros-args -r image:=/camera_2d_wide_full_hd/image -r camera/set_camera_info:=/camera_2d_wide_full_hd/set_camera_info
```
## B. sound_object_person_following
@@ -16,5 +18,5 @@ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.69 image:=/c
1. Place an object having visual features in front of the robot.
2. Execute the following command.
```bash
-roslaunch sound_object_person_following calibrate_sound_object_person_following.launch
+ros2 launch sound_object_person_following calibrate_sound_object_person_following.launch.xml
```
diff --git a/ros/.gitignore b/ros/.gitignore
deleted file mode 100644
index 0c03be98..00000000
--- a/ros/.gitignore
+++ /dev/null
@@ -1,3 +0,0 @@
-.idea/
-venv
-__pycache__
diff --git a/ros/behaviors/behavior_msgs/CMakeLists.txt b/ros/behaviors/behavior_msgs/CMakeLists.txt
new file mode 100644
index 00000000..35804804
--- /dev/null
+++ b/ros/behaviors/behavior_msgs/CMakeLists.txt
@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.5)
+project(behavior_msgs)
+
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(daemon_ros_client REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# Generate messages
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/Done.msg"
+ "msg/GestureName.msg"
+ "msg/LedAnimation.msg"
+ "msg/SoundFile.msg"
+ "msg/SoundStarted.msg"
+ "msg/Statistics.msg"
+ "msg/Text.msg"
+ DEPENDENCIES daemon_ros_client
+)
+
+ament_package()
diff --git a/ros/behaviors/gesture/msg/Done.msg b/ros/behaviors/behavior_msgs/msg/Done.msg
similarity index 100%
rename from ros/behaviors/gesture/msg/Done.msg
rename to ros/behaviors/behavior_msgs/msg/Done.msg
diff --git a/ros/behaviors/gesture/msg/GestureName.msg b/ros/behaviors/behavior_msgs/msg/GestureName.msg
similarity index 100%
rename from ros/behaviors/gesture/msg/GestureName.msg
rename to ros/behaviors/behavior_msgs/msg/GestureName.msg
diff --git a/ros/behaviors/led_animations/msg/Animation.msg b/ros/behaviors/behavior_msgs/msg/LedAnimation.msg
similarity index 100%
rename from ros/behaviors/led_animations/msg/Animation.msg
rename to ros/behaviors/behavior_msgs/msg/LedAnimation.msg
diff --git a/ros/behaviors/sound_player/msg/SoundFile.msg b/ros/behaviors/behavior_msgs/msg/SoundFile.msg
similarity index 100%
rename from ros/behaviors/sound_player/msg/SoundFile.msg
rename to ros/behaviors/behavior_msgs/msg/SoundFile.msg
diff --git a/ros/behaviors/sound_player/msg/Started.msg b/ros/behaviors/behavior_msgs/msg/SoundStarted.msg
similarity index 100%
rename from ros/behaviors/sound_player/msg/Started.msg
rename to ros/behaviors/behavior_msgs/msg/SoundStarted.msg
diff --git a/ros/behaviors/talk/msg/Statistics.msg b/ros/behaviors/behavior_msgs/msg/Statistics.msg
similarity index 100%
rename from ros/behaviors/talk/msg/Statistics.msg
rename to ros/behaviors/behavior_msgs/msg/Statistics.msg
diff --git a/ros/behaviors/talk/msg/Text.msg b/ros/behaviors/behavior_msgs/msg/Text.msg
similarity index 100%
rename from ros/behaviors/talk/msg/Text.msg
rename to ros/behaviors/behavior_msgs/msg/Text.msg
diff --git a/ros/behaviors/behavior_msgs/package.xml b/ros/behaviors/behavior_msgs/package.xml
new file mode 100644
index 00000000..278c7707
--- /dev/null
+++ b/ros/behaviors/behavior_msgs/package.xml
@@ -0,0 +1,24 @@
+
+
+
+ behavior_msgs
+ 0.0.0
+ The behavior_msgs package
+ Marc-Antoine Maheux
+ GPL-3.0 license
+
+ ament_cmake
+ rosidl_default_generators
+
+ daemon_ros_client
+
+ rosidl_default_runtime
+ rosidl_interface_packages
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/ros/behaviors/behavior_srvs/CMakeLists.txt b/ros/behaviors/behavior_srvs/CMakeLists.txt
new file mode 100644
index 00000000..b4b467bc
--- /dev/null
+++ b/ros/behaviors/behavior_srvs/CMakeLists.txt
@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.5)
+project(behavior_srvs)
+
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+# Generate services
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "srv/GenerateSpeechFromText.srv"
+)
+
+ament_package()
diff --git a/ros/behaviors/behavior_srvs/package.xml b/ros/behaviors/behavior_srvs/package.xml
new file mode 100644
index 00000000..5fe53c23
--- /dev/null
+++ b/ros/behaviors/behavior_srvs/package.xml
@@ -0,0 +1,22 @@
+
+
+
+ behavior_srvs
+ 0.0.0
+ The behavior_srvs package
+ Marc-Antoine Maheux
+ GPL-3.0 license
+
+ ament_cmake
+ rosidl_default_generators
+
+ rosidl_default_runtime
+ rosidl_interface_packages
+
+ ament_lint_auto
+ ament_lint_common
+
+
+ ament_cmake
+
+
diff --git a/ros/behaviors/piper_ros/srv/GenerateSpeechFromText.srv b/ros/behaviors/behavior_srvs/srv/GenerateSpeechFromText.srv
similarity index 100%
rename from ros/behaviors/piper_ros/srv/GenerateSpeechFromText.srv
rename to ros/behaviors/behavior_srvs/srv/GenerateSpeechFromText.srv
diff --git a/ros/behaviors/dance/CMakeLists.txt b/ros/behaviors/dance/CMakeLists.txt
index 6fbe000e..83246a06 100644
--- a/ros/behaviors/dance/CMakeLists.txt
+++ b/ros/behaviors/dance/CMakeLists.txt
@@ -1,221 +1,43 @@
-cmake_minimum_required(VERSION 2.8.3)
-
-if (POLICY CMP0048)
- cmake_policy(SET CMP0048 NEW)
-endif (POLICY CMP0048)
-
+cmake_minimum_required(VERSION 3.5)
project(dance)
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
-
-## 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
- rospy
- roscpp
- std_msgs
- hbba_lite
- t_top
- daemon_ros_client
- )
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 dance
- CATKIN_DEPENDS rospy roscpp std_msgs
- # DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
- # include
- ${catkin_INCLUDE_DIRS}
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(daemon_ros_client REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(t_top REQUIRED)
+
+# Python Librairies
+ament_python_install_package(${PROJECT_NAME})
+
+# Python Nodes
+install(PROGRAMS
+ scripts/head_dance_node.py
+ scripts/led_dance_node.py
+ scripts/torso_dance_node.py
+ DESTINATION lib/${PROJECT_NAME}
)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/dance.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/audio_utils_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(beat_detector_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-catkin_install_python(PROGRAMS
- scripts/head_dance_node.py
- scripts/torso_dance_node.py
- scripts/led_dance_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-## 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 for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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 ##
-#############
+# Install the movements files
+install(DIRECTORY movements DESTINATION share/${PROJECT_NAME})
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_dance.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+# Launch files
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ament_package()
diff --git a/ros/behaviors/dance/README.md b/ros/behaviors/dance/README.md
index 3e671ee1..d5ec8661 100644
--- a/ros/behaviors/dance/README.md
+++ b/ros/behaviors/dance/README.md
@@ -14,19 +14,19 @@ This node makes T-Top head dance on the beat. The moves are randomly chosen.
#### Subscribed Topics
-- `bpm` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)): The bpm topic from the
+- `bpm` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)): The bpm topic from the
beat_detector_node.
-- `beat` ([std_msgs/Bool](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html)): The beat topic from the
+- `beat` ([std_msgs/Bool](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Bool.html)): The beat topic from the
beat_detector_node.
#### Published Topics
-- `dance/set_head_pose` ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)):
+- `dance/set_head_pose` ([geometry_msgs/PoseStamped](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/PoseStamped.html)):
The head pose.
#### Services
-- `pose/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `pose/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
@@ -40,20 +40,20 @@ This node makes T-Top torso dance on the beat. The moves are randomly chosen.
#### Subscribed Topics
-- `bpm` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)): The bpm topic from the
+- `bpm` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)): The bpm topic from the
beat_detector_node.
-- `beat` ([std_msgs/Bool](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html)): The beat topic from the
+- `beat` ([std_msgs/Bool](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Bool.html)): The beat topic from the
beat_detector_node.
- `daemon/motor_status` ([daemon_ros_client/MotorStatus](../../daemon_ros_client/msg/MotorStatus.msg)): The motor status to get torso orientation.
#### Published Topics
-- `dance/set_torso_orientation` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)): The
+- `dance/set_torso_orientation` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)): The
torso orientation.
#### Services
-- `pose/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `pose/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
@@ -67,7 +67,7 @@ This node makes T-Top torso dance on the beat. The moves are randomly chosen.
#### Subscribed Topics
-- `beat` ([std_msgs/Bool](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Bool.html)): The beat topic from the
+- `beat` ([std_msgs/Bool](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Bool.html)): The beat topic from the
beat_detector_node.
#### Published Topics
@@ -77,5 +77,5 @@ This node makes T-Top torso dance on the beat. The moves are randomly chosen.
#### Services
-- `set_led_colors/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `set_led_colors/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
diff --git a/ros/behaviors/face_following/src/face_following/__init__.py b/ros/behaviors/dance/dance/__init__.py
similarity index 100%
rename from ros/behaviors/face_following/src/face_following/__init__.py
rename to ros/behaviors/dance/dance/__init__.py
diff --git a/ros/behaviors/dance/src/dance/lib_pose_dance_node.py b/ros/behaviors/dance/dance/lib_pose_dance_node.py
similarity index 51%
rename from ros/behaviors/dance/src/dance/lib_pose_dance_node.py
rename to ros/behaviors/dance/dance/lib_pose_dance_node.py
index b06b09b5..c8a4985e 100755
--- a/ros/behaviors/dance/src/dance/lib_pose_dance_node.py
+++ b/ros/behaviors/dance/dance/lib_pose_dance_node.py
@@ -1,11 +1,13 @@
#!/usr/bin/env python3
from abc import ABC, abstractmethod
-import threading
import json
import random
-import rospy
+import rclpy
+import rclpy.node
+from rclpy.duration import Duration
+
from std_msgs.msg import Float32
from std_msgs.msg import Bool
@@ -15,39 +17,38 @@
P_CHANGE_MOVEMENT = 0.25
-class PoseDanceNode(ABC):
- def __init__(self):
- self._lock = threading.Lock()
+class PoseDanceNode(rclpy.node.Node, ABC):
+ def __init__(self, node_name):
+ super(PoseDanceNode, self).__init__(node_name)
- with open(rospy.get_param('~movement_file'), 'r') as f:
+ movement_file = self.declare_parameter('movement_file', '').get_parameter_value().string_value
+ with open(movement_file, 'r') as f:
self._movements = json.load(f)
self._current_movement = self._movements[list(self._movements.keys())[0]]
self._current_movement_pose_index = 0
- self._last_pose_time = rospy.Time.now()
+ self._last_pose_time = self.get_clock().now()
self._bpm = 120
self._peak_delay_pose_count = 0
- self._rate = rospy.Rate(100)
+ self._time = self.create_timer(1 / 100, self._timer_callback)
- self._hbba_filter_state = hbba_lite.OnOffHbbaFilterState('pose/filter_state')
+ self._hbba_filter_state = hbba_lite.OnOffHbbaFilterState(self, 'pose/filter_state')
self._hbba_filter_state.on_changed(self._hbba_filter_state_cb)
- self._bpm_sub = rospy.Subscriber('bpm', Float32, self._bpm_cb, queue_size=1)
- self._beat_sub = rospy.Subscriber('beat', Bool, self._beat_cb, queue_size=1)
+ self._bpm_sub = self.create_subscription(Float32, 'bpm', self._bpm_cb, 1)
+ self._beat_sub = self.create_subscription(Bool, 'beat', self._beat_cb, 1)
def _hbba_filter_state_cb(self, previous_is_filtering_all_messages, new_is_filtering_all_messages):
pass
def _bpm_cb(self, msg):
if msg.data > 0:
- with self._lock:
- self._bpm = msg.data
+ self._bpm = msg.data
def _beat_cb(self, msg):
if msg.data:
- with self._lock:
- self._peak_delay_pose_count = self._find_peak_delay_pose_count()
+ self._peak_delay_pose_count = self._find_peak_delay_pose_count()
def _find_peak_delay_pose_count(self):
relative_index = float(self._current_movement_pose_index) / (len(self._current_movement['poses']) - 1)
@@ -56,24 +57,23 @@ def _find_peak_delay_pose_count(self):
return (closest_value - relative_index) * (len(self._current_movement['poses']) - 1)
- def run(self):
- while not rospy.is_shutdown():
- with self._lock:
- if not self._hbba_filter_state.is_filtering_all_messages:
- pose_duration = self._get_pose_duration()
- if (rospy.Time.now() - self._last_pose_time) > pose_duration:
- self._send_pose(self._current_movement['poses'][self._current_movement_pose_index])
- self._current_movement_pose_index += 1
- self._last_pose_time = rospy.Time.now()
-
- if self._current_movement_pose_index == len(self._current_movement['poses']):
- self._current_movement_pose_index = 0
- self._update_movement()
- else:
+ def _timer_callback(self):
+ if not self._hbba_filter_state.is_filtering_all_messages:
+ pose_duration = self._get_pose_duration()
+ if (self.get_clock().now() - self._last_pose_time) > pose_duration:
+ self._send_pose(self._current_movement['poses'][self._current_movement_pose_index])
+ self._current_movement_pose_index += 1
+ self._last_pose_time = self.get_clock().now()
+
+ if self._current_movement_pose_index == len(self._current_movement['poses']):
self._current_movement_pose_index = 0
- self._last_pose_time = rospy.Time.now()
+ self._update_movement()
+ else:
+ self._current_movement_pose_index = 0
+ self._last_pose_time = self.get_clock().now()
- self._rate.sleep()
+ def run(self):
+ rclpy.spin(self)
def _update_movement(self):
if random.random() < P_CHANGE_MOVEMENT:
@@ -85,22 +85,9 @@ def _get_pose_duration(self):
beat_duration = self._current_movement['beat_duration']
pose_count = len(self._current_movement['poses']) + self._peak_delay_pose_count
- return rospy.Duration.from_sec(float(beat_duration) / pose_count / self._bpm * 60.0)
+ return Duration(seconds=float(beat_duration) / pose_count / self._bpm * 60.0)
@abstractmethod
def _send_pose(self, pose):
""" Called with self._lock locked """
pass
-
-
-def main():
- rospy.init_node('dance_node')
- dance_node = DanceNode()
- dance_node.run()
-
-
-if __name__ == '__main__':
- try:
- main()
- except rospy.ROSInterruptException:
- pass
diff --git a/ros/behaviors/dance/launch/test_dance.launch.xml b/ros/behaviors/dance/launch/test_dance.launch.xml
new file mode 100644
index 00000000..0891b933
--- /dev/null
+++ b/ros/behaviors/dance/launch/test_dance.launch.xml
@@ -0,0 +1,53 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ros/behaviors/dance/head_movements.json b/ros/behaviors/dance/movements/head_movements.json
similarity index 100%
rename from ros/behaviors/dance/head_movements.json
rename to ros/behaviors/dance/movements/head_movements.json
diff --git a/ros/behaviors/dance/led_colors.json b/ros/behaviors/dance/movements/led_colors.json
similarity index 100%
rename from ros/behaviors/dance/led_colors.json
rename to ros/behaviors/dance/movements/led_colors.json
diff --git a/ros/behaviors/dance/torso_movements.json b/ros/behaviors/dance/movements/torso_movements.json
similarity index 100%
rename from ros/behaviors/dance/torso_movements.json
rename to ros/behaviors/dance/movements/torso_movements.json
diff --git a/ros/behaviors/dance/package.xml b/ros/behaviors/dance/package.xml
index f7134ee3..7c9e0b4f 100644
--- a/ros/behaviors/dance/package.xml
+++ b/ros/behaviors/dance/package.xml
@@ -1,77 +1,28 @@
-
+
+
dance
0.0.0
The dance package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- mahm1904
+ ament_cmake
+ rclpy
+ daemon_ros_client
+ std_msgs
+ hbba_lite
+ t_top
-
-
-
- TODO
+ rosidl_default_runtime
+ ros2launch
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- rospy
- roscpp
- std_msgs
- hbba_lite
- t_top
- daemon_ros_client
- rospy
- roscpp
- std_msgs
- hbba_lite
- t_top
- daemon_ros_client
- rospy
- roscpp
- std_msgs
- hbba_lite
- t_top
- daemon_ros_client
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/dance/scripts/head_dance_node.py b/ros/behaviors/dance/scripts/head_dance_node.py
index a6aa7fd0..3e3ff38b 100755
--- a/ros/behaviors/dance/scripts/head_dance_node.py
+++ b/ros/behaviors/dance/scripts/head_dance_node.py
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
-import rospy
+import rclpy
+
from geometry_msgs.msg import PoseStamped
from t_top import HEAD_ZERO_Z
@@ -8,8 +9,8 @@
class HeadDanceNode(PoseDanceNode):
def __init__(self):
- self._head_pose_pub = rospy.Publisher('dance/set_head_pose', PoseStamped, queue_size=5)
- super(HeadDanceNode, self).__init__()
+ super().__init__('head_dance_node')
+ self._head_pose_pub = self.create_publisher(PoseStamped, 'dance/set_head_pose', 5)
def _send_pose(self, pose):
""" Called with self._lock locked """
@@ -17,28 +18,33 @@ def _send_pose(self, pose):
pose_msg = PoseStamped()
pose_msg.header.frame_id = 'stewart_base'
- pose_msg.pose.position.x = pose[0]
- pose_msg.pose.position.y = pose[1]
- pose_msg.pose.position.z = HEAD_ZERO_Z + pose[2]
+ pose_msg.pose.position.x = float(pose[0])
+ pose_msg.pose.position.y = float(pose[1])
+ pose_msg.pose.position.z = float(HEAD_ZERO_Z + pose[2])
- pose_msg.pose.orientation.x = pose[3]
- pose_msg.pose.orientation.y = pose[4]
- pose_msg.pose.orientation.z = pose[5]
- pose_msg.pose.orientation.w = pose[6]
+ pose_msg.pose.orientation.x = float(pose[3])
+ pose_msg.pose.orientation.y = float(pose[4])
+ pose_msg.pose.orientation.z = float(pose[5])
+ pose_msg.pose.orientation.w = float(pose[6])
self._head_pose_pub.publish(pose_msg)
else:
- rospy.logerr('Invalid pose')
+ self.get_logger().error('Invalid pose')
def main():
- rospy.init_node('head_dance_node')
+ rclpy.init()
head_dance_node = HeadDanceNode()
- head_dance_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ head_dance_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ head_dance_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/dance/scripts/led_dance_node.py b/ros/behaviors/dance/scripts/led_dance_node.py
old mode 100644
new mode 100755
index 010d59ca..ef5a1879
--- a/ros/behaviors/dance/scripts/led_dance_node.py
+++ b/ros/behaviors/dance/scripts/led_dance_node.py
@@ -1,7 +1,10 @@
+#!/usr/bin/env python3
+
import json
import random
-import rospy
+import rclpy
+import rclpy.node
from std_msgs.msg import Bool
from daemon_ros_client.msg import LedColors
@@ -9,16 +12,19 @@
import hbba_lite
-class LedDanceNode:
+class LedDanceNode(rclpy.node.Node):
def __init__(self):
- with open(rospy.get_param('~led_colors_file'), 'r') as f:
+ super().__init__('led_dance_node')
+
+ led_colors_file = self.declare_parameter('led_colors_file', '').get_parameter_value().string_value
+ with open(led_colors_file, 'r') as f:
self._none_led_colors, self._dance_led_colors = self._load_led_colors(json.load(f))
- self._led_colors_pub = hbba_lite.OnOffHbbaPublisher('dance/set_led_colors', LedColors, queue_size=1,
+ self._led_colors_pub = hbba_lite.OnOffHbbaPublisher(self, LedColors, 'dance/set_led_colors', 1,
state_service_name='set_led_colors/filter_state')
self._led_colors_pub.on_filter_state_changing(self._hbba_filter_state_cb)
- self._beat_sub = rospy.Subscriber('beat', Bool, self._beat_cb, queue_size=1)
+ self._beat_sub = self.create_subscription(Bool, 'beat', self._beat_cb, 1)
def _load_led_colors(self, json_dict):
none_led_colors = LedColors()
@@ -49,17 +55,22 @@ def _beat_cb(self, msg):
self._led_colors_pub.publish(random.choice(self._dance_led_colors))
def run(self):
- rospy.spin()
+ rclpy.spin(self)
def main():
- rospy.init_node('led_dance_node')
+ rclpy.init()
led_dance_node = LedDanceNode()
- led_dance_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ led_dance_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ led_dance_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/dance/scripts/torso_dance_node.py b/ros/behaviors/dance/scripts/torso_dance_node.py
index 7189f92d..a1177ac4 100755
--- a/ros/behaviors/dance/scripts/torso_dance_node.py
+++ b/ros/behaviors/dance/scripts/torso_dance_node.py
@@ -1,57 +1,54 @@
#!/usr/bin/env python3
-import threading
+import rclpy
-import rospy
from std_msgs.msg import Float32
from daemon_ros_client.msg import MotorStatus
from dance.lib_pose_dance_node import PoseDanceNode
-P_CHANGE_MOVEMENT = 0.25
-
-
class TorsoDanceNode(PoseDanceNode):
def __init__(self):
- self._current_torso_orientation_lock = threading.Lock()
+ super().__init__('torso_dance_node')
+
self._current_torso_orientation = 0.0
self._torso_offset = 0.0
- self._torso_orientation_pub = rospy.Publisher('dance/set_torso_orientation', Float32, queue_size=5)
+ self._torso_orientation_pub = self.create_publisher(Float32, 'dance/set_torso_orientation', 5)
- super(TorsoDanceNode, self).__init__()
-
- self._motor_status_sub = rospy.Subscriber('daemon/motor_status', MotorStatus, self._motor_status_cb, queue_size=1)
+ self._motor_status_sub = self.create_subscription(MotorStatus, 'daemon/motor_status', self._motor_status_cb, 1)
def _hbba_filter_state_cb(self, previous_is_filtering_all_messages, new_is_filtering_all_messages):
if previous_is_filtering_all_messages and not new_is_filtering_all_messages:
- with self._lock, self._current_torso_orientation_lock:
- self._torso_offset = self._current_torso_orientation
+ self._torso_offset = self._current_torso_orientation
def _motor_status_cb(self, msg):
- with self._current_torso_orientation_lock:
- self._current_torso_orientation = msg.torso_orientation
+ self._current_torso_orientation = msg.torso_orientation
def _send_pose(self, pose):
- """ Called with self._lock locked """
if len(pose) == 1:
pose_msg = Float32()
- pose_msg.data = pose[0] + self._torso_offset
+ pose_msg.data = float(pose[0] + self._torso_offset)
self._torso_orientation_pub.publish(pose_msg)
else:
- rospy.logerr('Invalid pose')
+ self.get_logger().error('Invalid pose')
def main():
- rospy.init_node('torso_dance_node')
+ rclpy.init()
torso_dance_node = TorsoDanceNode()
- torso_dance_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ torso_dance_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ torso_dance_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/dance/setup.py b/ros/behaviors/dance/setup.py
deleted file mode 100644
index 250d9983..00000000
--- a/ros/behaviors/dance/setup.py
+++ /dev/null
@@ -1,12 +0,0 @@
-## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from setuptools import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
- packages=['dance'],
- package_dir={'': 'src'},
-)
-
-setup(**setup_args)
diff --git a/ros/behaviors/explore/CMakeLists.txt b/ros/behaviors/explore/CMakeLists.txt
index 32770381..35e8da62 100644
--- a/ros/behaviors/explore/CMakeLists.txt
+++ b/ros/behaviors/explore/CMakeLists.txt
@@ -1,207 +1,32 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(explore)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## 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
- hbba_lite
- rospy
- t_top
- std_msgs
- )
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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 # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 explore
- # CATKIN_DEPENDS hbba_lite rospy t_top
- # DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
- # include
- ${catkin_INCLUDE_DIRS}
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(daemon_ros_client REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(t_top REQUIRED)
+
+# Python Nodes
+install(PROGRAMS
+ scripts/explore_node.py
+ DESTINATION lib/${PROJECT_NAME}
)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/explore.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/explore_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_explore.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_package()
diff --git a/ros/behaviors/explore/README.md b/ros/behaviors/explore/README.md
index b4787c5a..18658c3c 100644
--- a/ros/behaviors/explore/README.md
+++ b/ros/behaviors/explore/README.md
@@ -10,10 +10,10 @@ This node makes T-Top explore its surrounding.
#### Parameters
-- `simulation` (bool): Indicates if it's used in the simulation.
-- `explore_frequency` (double): The frequency at which T-Top explores its surrounding.
-- `torso_speed_rad_sec` (double): The torso speed in rad/s.
-- `head_speed_rad_sec` (double): The head speed in rad/s.
+- `simulation` (bool): Indicates if it's used in the simulation. The default value is false.
+- `explore_frequency` (double): The frequency at which T-Top explores its surrounding. The default value is 0.00833333333.
+- `torso_speed_rad_sec` (double): The torso speed in rad/s. The default value is 0.5.
+- `head_speed_rad_sec` (double): The head speed in rad/s. The default value is 0.5.
#### Subscribed Topics
@@ -21,14 +21,14 @@ This node makes T-Top explore its surrounding.
#### Published Topics
-- `explore/set_head_pose` ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)):
+- `explore/set_head_pose` ([geometry_msgs/PoseStamped](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/PoseStamped.html)):
The head pose.
-- `explore/set_torso_orientation` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)): The
+- `explore/set_torso_orientation` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)): The
torso orientation.
-- `explore/done` ([std_msgs/Empty](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Empty.html)): Indicates that the
+- `explore/done` ([std_msgs/Empty](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Empty.html)): Indicates that the
explore moves are finished.
#### Services
-- `pose/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `pose/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
diff --git a/ros/behaviors/explore/package.xml b/ros/behaviors/explore/package.xml
index d9cff3f7..885224ee 100644
--- a/ros/behaviors/explore/package.xml
+++ b/ros/behaviors/explore/package.xml
@@ -1,71 +1,27 @@
-
+
+
explore
0.0.0
The explore package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclpy
+ daemon_ros_client
+ std_msgs
+ hbba_lite
+ t_top
-
-
-
- TODO
+ rosidl_default_runtime
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- hbba_lite
- rospy
- t_top
- std_msgs
- hbba_lite
- rospy
- t_top
- std_msgs
- hbba_lite
- rospy
- t_top
- std_msgs
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/explore/scripts/explore_node.py b/ros/behaviors/explore/scripts/explore_node.py
index 44af2f98..5667a89d 100755
--- a/ros/behaviors/explore/scripts/explore_node.py
+++ b/ros/behaviors/explore/scripts/explore_node.py
@@ -1,6 +1,9 @@
#!/usr/bin/env python3
-import rospy
+import rclpy
+import rclpy.node
+import rclpy.executors
+
from std_msgs.msg import Empty
from t_top import MovementCommands, HEAD_ZERO_Z
@@ -9,49 +12,59 @@
INACTIVE_SLEEP_DURATION = 0.1
-class ExploreNode:
+class ExploreNode(rclpy.node.Node):
def __init__(self):
- self._simulation = rospy.get_param('~simulation')
- self._rate = rospy.Rate(rospy.get_param('~explore_frequency'))
- self._torso_speed = rospy.get_param('~torso_speed_rad_sec')
- self._head_speed = rospy.get_param('~head_speed_rad_sec')
+ super().__init__('explore_node')
+
+ self._simulation = self.declare_parameter('simulation', False).get_parameter_value().bool_value
+ self._explore_frequency = self.declare_parameter('explore_frequency', 0.00833333333).get_parameter_value().double_value
+ self._torso_speed = self.declare_parameter('torso_speed_rad_sec', 0.5).get_parameter_value().double_value
+ self._head_speed = self.declare_parameter('head_speed_rad_sec', 0.5).get_parameter_value().double_value
+
+ self._timer = self.create_timer(1 / self._explore_frequency, self._timer_callback)
+
+ self._movement_commands = MovementCommands(self, self._simulation, namespace='explore')
+ self._done_pub = self.create_publisher(Empty, 'explore/done', 5)
+
+ def _timer_callback(self):
+ if self._movement_commands.is_filtering_all_messages:
+ return
+
+ self._movement_commands.move_torso(0, should_wait=True, speed_rad_sec=self._torso_speed)
+ self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, -0.3, 0], should_wait=True, speed_rad_sec=self._head_speed)
+ self._movement_commands.move_torso(1.57, should_wait=False, speed_rad_sec=self._torso_speed)
+ self._movement_commands.move_torso(3.14, should_wait=True, speed_rad_sec=self._torso_speed)
+ self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, 0.15, 0], should_wait=True, speed_rad_sec=self._head_speed)
+ self._movement_commands.move_torso(1.57, should_wait=False, speed_rad_sec=self._torso_speed)
+ self._movement_commands.move_torso(0, should_wait=False, speed_rad_sec=self._torso_speed)
+ self._movement_commands.move_torso(-1.57, should_wait=False, speed_rad_sec=self._torso_speed)
+ self._movement_commands.move_torso(-3.14, should_wait=True, speed_rad_sec=self._torso_speed)
+ self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, -0.3, 0], should_wait=True, speed_rad_sec=self._head_speed)
+ self._movement_commands.move_torso(-1.57, should_wait=False, speed_rad_sec=self._torso_speed)
+ self._movement_commands.move_torso(0, should_wait=True, speed_rad_sec=self._torso_speed)
+ self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, 0, 0], should_wait=True, speed_rad_sec=self._head_speed)
- self._movement_commands = MovementCommands(self._simulation, namespace='explore')
- self._done_pub = rospy.Publisher('explore/done', Empty, queue_size=5)
+ self._done_pub.publish(Empty())
def run(self):
- while not rospy.is_shutdown():
- if self._movement_commands.is_filtering_all_messages:
- rospy.sleep(INACTIVE_SLEEP_DURATION)
- continue
-
- self._movement_commands.move_torso(0, should_wait=True, speed_rad_sec=self._torso_speed)
- self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, -0.3, 0], should_wait=True, speed_rad_sec=self._head_speed)
- self._movement_commands.move_torso(1.57, should_wait=False, speed_rad_sec=self._torso_speed)
- self._movement_commands.move_torso(3.14, should_wait=True, speed_rad_sec=self._torso_speed)
- self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, 0.15, 0], should_wait=True, speed_rad_sec=self._head_speed)
- self._movement_commands.move_torso(1.57, should_wait=False, speed_rad_sec=self._torso_speed)
- self._movement_commands.move_torso(0, should_wait=False, speed_rad_sec=self._torso_speed)
- self._movement_commands.move_torso(-1.57, should_wait=False, speed_rad_sec=self._torso_speed)
- self._movement_commands.move_torso(-3.14, should_wait=True, speed_rad_sec=self._torso_speed)
- self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, -0.3, 0], should_wait=True, speed_rad_sec=self._head_speed)
- self._movement_commands.move_torso(-1.57, should_wait=False, speed_rad_sec=self._torso_speed)
- self._movement_commands.move_torso(0, should_wait=True, speed_rad_sec=self._torso_speed)
- self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, 0, 0], should_wait=True, speed_rad_sec=self._head_speed)
-
- self._done_pub.publish(Empty())
-
- self._rate.sleep()
+ executor = rclpy.executors.MultiThreadedExecutor(num_threads=2)
+ executor.add_node(self)
+ executor.spin()
def main():
- rospy.init_node('explore_node')
+ rclpy.init()
explore_node = ExploreNode()
- explore_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ explore_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ explore_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/face_following/CMakeLists.txt b/ros/behaviors/face_following/CMakeLists.txt
index b8336576..e5e87952 100644
--- a/ros/behaviors/face_following/CMakeLists.txt
+++ b/ros/behaviors/face_following/CMakeLists.txt
@@ -1,212 +1,40 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(face_following)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-## 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
- std_msgs
- geometry_msgs
- hbba_lite
- rospy
- t_top
- tf
- video_analyzer
- person_identification
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(t_top REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(perception_msgs REQUIRED)
+
+# Python Librairies
+ament_python_install_package(${PROJECT_NAME})
+
+# Python Nodes
+install(PROGRAMS
+ scripts/nearest_face_following_node.py
+ scripts/specific_face_following_node.py
+ DESTINATION lib/${PROJECT_NAME}
)
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
-# geometry_msgs# std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 face_following
- # CATKIN_DEPENDS geometry_msgs hbba_lite rospy std_msgs t_top tf
- # DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
- # include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/face_following.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/face_following_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-catkin_install_python(PROGRAMS
- scripts/nearest_face_following_node.py
- scripts/specific_face_following_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_face_following.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_package()
diff --git a/ros/behaviors/face_following/README.md b/ros/behaviors/face_following/README.md
index b97c10ff..4c1b08b7 100644
--- a/ros/behaviors/face_following/README.md
+++ b/ros/behaviors/face_following/README.md
@@ -10,31 +10,31 @@ This node makes T-Top follow the nearest face.
#### Parameters
-- `simulation` (bool): Indicates if it's used in the simulation.
-- `control_frequency` (double): The frequency at which the pose messages are sent.
-- `torso_control_alpha` (double): The low-pass filter parameter for the torso pose.
-- `head_control_p_gain` (double): The controller proportional gain for the head pose.
-- `head_enabled` (bool): Indicates if the head will move.
-- `min_head_pitch_rad` (double): The minimum pitch angle in radian of the head.
-- `max_head_pitch_rad` (double): The maximum pitch angle in radian of the head.
-- `nose_confidence_threshold` (double): The confidence threshold for the nose keypoint.
+- `simulation` (bool): Indicates if it's used in the simulation. The default value is false.
+- `control_frequency` (double): The frequency at which the pose messages are sent. The default value is 30.
+- `torso_control_alpha` (double): The low-pass filter parameter for the torso pose. The default value is 0.2.
+- `head_control_p_gain` (double): The controller proportional gain for the head pose. The default value is 0.175.
+- `head_enabled` (bool): Indicates if the head will move. The default value is true.
+- `min_head_pitch_rad` (double): The minimum pitch angle in radian of the head. The default value is -0.35.
+- `max_head_pitch_rad` (double): The maximum pitch angle in radian of the head. The default value is 0.35.
+- `nose_confidence_threshold` (double): The confidence threshold for the nose keypoint. The default value is 0.4.
#### Subscribed Topics
- `daemon/motor_status` ([daemon_ros_client/MotorStatus](../../daemon_ros_client/msg/MotorStatus.msg)): The motor status.
-- `video_analysis` ([video_analyzer/VideoAnalysis](../../perceptions/video_analyzer/msg/VideoAnalysis.msg)): The video
+- `video_analysis` ([perception_msgs/VideoAnalysis](../../perceptions/perception_msgs/msg/VideoAnalysis.msg)): The video
analysis containing the detected objects. The video analysis must contain 3d positions.
#### Published Topics
-- `nearest_face_following/set_head_pose` ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)):
+- `nearest_face_following/set_head_pose` ([geometry_msgs/PoseStamped](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/PoseStamped.html)):
The head pose.
-- `nearest_face_following/set_torso_orientation` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)):
+- `nearest_face_following/set_torso_orientation` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)):
The torso orientation.
#### Services
-- `pose/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `pose/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
@@ -44,29 +44,29 @@ This node makes T-Top follow the face of the specified person.
#### Parameters
-- `simulation` (bool): Indicates if it's used in the simulation.
-- `control_frequency` (double): The frequency at which the pose messages are sent.
-- `torso_control_alpha` (double): The low-pass filter parameter for the torso pose.
-- `head_control_p_gain` (double): The controller proportional gain for the head pose.
-- `head_enabled` (bool): Indicates if the head will move.
-- `min_head_pitch_rad` (double): The minimum pitch angle in radian of the head.
-- `max_head_pitch_rad` (double): The maximum pitch angle in radian of the head.
-- `direction_frame_id` (string): The audio analysis frame id.
+- `simulation` (bool): Indicates if it's used in the simulation. The default value is false.
+- `control_frequency` (double): The frequency at which the pose messages are sent. The default value is 30.
+- `torso_control_alpha` (double): The low-pass filter parameter for the torso pose. The default value is 0.2.
+- `head_control_p_gain` (double): The controller proportional gain for the head pose. The default value is 0.175.
+- `head_enabled` (bool): Indicates if the head will move. The default value is true.
+- `min_head_pitch_rad` (double): The minimum pitch angle in radian of the head. The default value is -0.35.
+- `max_head_pitch_rad` (double): The maximum pitch angle in radian of the head. The default value is 0.35.
+- `direction_frame_id` (string): The audio analysis frame id. The default value is odas.
#### Subscribed Topics
-- `person_names` ([person_identification/PersonNames](../../perceptions/person_identification/msg/PersonNames.msg)):
+- `person_names` ([person_identification/PersonNames](../../perceptions/perception_msgs/msg/PersonNames.msg)):
The person names.
- `daemon/motor_status` ([daemon_ros_client/MotorStatus](../../daemon_ros_client/msg/MotorStatus.msg)): The motor status.
#### Published Topics
-- `specific_face_following/set_head_pose` ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)):
+- `specific_face_following/set_head_pose` ([geometry_msgs/PoseStamped](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/PoseStamped.html)):
The head pose.
-- `specific_face_following/set_torso_orientation` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)):
+- `specific_face_following/set_torso_orientation` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)):
The torso orientation.
#### Services
-- `pose/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `pose/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
diff --git a/ros/behaviors/led_animations/src/led_animations/__init__.py b/ros/behaviors/face_following/face_following/__init__.py
similarity index 100%
rename from ros/behaviors/led_animations/src/led_animations/__init__.py
rename to ros/behaviors/face_following/face_following/__init__.py
diff --git a/ros/behaviors/face_following/face_following/lib_face_following_node.py b/ros/behaviors/face_following/face_following/lib_face_following_node.py
new file mode 100644
index 00000000..1a643bab
--- /dev/null
+++ b/ros/behaviors/face_following/face_following/lib_face_following_node.py
@@ -0,0 +1,71 @@
+import math
+
+import rclpy
+import rclpy.node
+import rclpy.executors
+
+from t_top import MovementCommands, HEAD_ZERO_Z, HEAD_POSE_PITCH_INDEX
+
+
+TARGET_HEAD_IMAGE_Y = 0.5
+
+
+class FaceFollowingNode(rclpy.node.Node):
+ def __init__(self, node_name, namespace):
+ super().__init__(node_name)
+
+ self._simulation = self.declare_parameter('simulation', False).get_parameter_value().bool_value
+ self._control_frequency = self.declare_parameter('control_frequency', 30.0).get_parameter_value().double_value
+ self._torso_control_alpha = self.declare_parameter('torso_control_alpha', 0.2).get_parameter_value().double_value
+ self._head_control_p_gain = self.declare_parameter('head_control_p_gain', 0.175).get_parameter_value().double_value
+ self._head_enabled = self.declare_parameter('head_enabled', True).get_parameter_value().bool_value
+ self._min_head_pitch = self.declare_parameter('min_head_pitch_rad', -0.35).get_parameter_value().double_value
+ self._max_head_pitch = self.declare_parameter('max_head_pitch_rad', 0.35).get_parameter_value().double_value
+
+ self._target_torso_yaw = None
+ self._current_head_image_y = None
+
+ self._movement_commands = MovementCommands(self, self._simulation, namespace)
+
+ self._timer = self.create_timer(1 / self._control_frequency, self._timer_callback)
+
+ def _update(self, yaw, head_image_y):
+ if yaw is None or math.isfinite(yaw):
+ self._target_torso_yaw = yaw
+ if head_image_y is None or math.isfinite(head_image_y):
+ self._current_head_image_y = head_image_y
+
+ def _timer_callback(self):
+ if self._movement_commands.is_filtering_all_messages:
+ return
+
+ self._update_torso()
+ if self._head_enabled:
+ self._update_head()
+
+ def run(self):
+ executor = rclpy.executors.MultiThreadedExecutor(num_threads=2)
+ executor.add_node(self)
+ executor.spin()
+
+ def _update_torso(self):
+ if self._target_torso_yaw is None:
+ return
+
+ distance = self._target_torso_yaw - self._movement_commands.current_torso_pose
+ if distance < -math.pi:
+ distance = 2 * math.pi + distance
+ elif distance > math.pi:
+ distance = -(2 * math.pi - distance)
+
+ pose = self._movement_commands.current_torso_pose + self._torso_control_alpha * distance
+ self._movement_commands.move_torso(pose)
+
+ def _update_head(self):
+ if self._current_head_image_y is None:
+ return
+
+ current_pitch = self._movement_commands.current_head_pose[HEAD_POSE_PITCH_INDEX]
+ pitch = current_pitch + self._head_control_p_gain * (self._current_head_image_y - TARGET_HEAD_IMAGE_Y)
+ pitch = max(self._min_head_pitch, min(pitch, self._max_head_pitch))
+ self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, pitch, 0])
diff --git a/ros/behaviors/face_following/package.xml b/ros/behaviors/face_following/package.xml
index ca0a9fda..d10e7e65 100644
--- a/ros/behaviors/face_following/package.xml
+++ b/ros/behaviors/face_following/package.xml
@@ -1,83 +1,30 @@
-
+
+
face_following
0.0.0
The face_following package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclpy
+ std_msgs
+ geometry_msgs
+ hbba_lite
+ t_top
+ tf2
+ tf2_ros
+ perception_msgs
-
-
-
- TODO
+ rosidl_default_runtime
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- std_msgs
- geometry_msgs
- hbba_lite
- rospy
- t_top
- tf
- video_analyzer
- person_identification
- std_msgs
- geometry_msgs
- hbba_lite
- rospy
- t_top
- tf
- video_analyzer
- person_identification
- std_msgs
- geometry_msgs
- hbba_lite
- rospy
- t_top
- tf
- video_analyzer
- person_identification
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/face_following/scripts/nearest_face_following_node.py b/ros/behaviors/face_following/scripts/nearest_face_following_node.py
index a3f464dc..7561d086 100755
--- a/ros/behaviors/face_following/scripts/nearest_face_following_node.py
+++ b/ros/behaviors/face_following/scripts/nearest_face_following_node.py
@@ -1,11 +1,16 @@
#!/usr/bin/env python3
-import rospy
-import tf
+import rclpy
+
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+import tf2_geometry_msgs
+
from geometry_msgs.msg import PointStamped
-from video_analyzer.msg import VideoAnalysis
+from perception_msgs.msg import VideoAnalysis
-from t_top import vector_to_angles, HEAD_ZERO_Z
+from t_top import vector_to_angles
from face_following.lib_face_following_node import FaceFollowingNode
@@ -15,21 +20,26 @@
class NearestFaceFollowingNode(FaceFollowingNode):
def __init__(self):
- super().__init__(namespace='nearest_face_following')
- self._nose_confidence_threshold = rospy.get_param('~nose_confidence_threshold')
+ super().__init__(node_name='nearest_face_following_node', namespace='nearest_face_following')
+ self._nose_confidence_threshold = self.declare_parameter('nose_confidence_threshold', 0.4).get_parameter_value().double_value
- self._tf_listener = tf.TransformListener()
- self._video_analysis_sub = rospy.Subscriber('video_analysis', VideoAnalysis, self._video_analysis_cb, queue_size=1)
+ self._tf_buffer = Buffer()
+ self._tf_listener = TransformListener(self._tf_buffer, self)
+
+ self._video_analysis_sub = self.create_subscription(VideoAnalysis, 'video_analysis', self._video_analysis_cb, 1)
def _video_analysis_cb(self, msg):
if self._movement_commands.is_filtering_all_messages:
return
if not msg.contains_3d_positions:
- rospy.logerr('The video analysis must contain 3d positions.')
+ self.get_logger().error('The video analysis must contain 3d positions.')
return
- yaw, head_image_y = self._find_nearest_face_yaw_head_image_y(msg.objects, msg.header)
- self._update(yaw, head_image_y)
+ try:
+ yaw, head_image_y = self._find_nearest_face_yaw_head_image_y(msg.objects, msg.header)
+ self._update(yaw, head_image_y)
+ except TransformException as ex:
+ self.get_logger().warn(f'Could not transform: {ex}')
def _find_nearest_face_yaw_head_image_y(self, objects, header):
nose_points_3d = []
@@ -57,22 +67,27 @@ def _transform_point(self, point, header):
temp_in_point.point.y = point.y
temp_in_point.point.z = point.z
- base_link_point = self._tf_listener.transformPoint('/base_link', temp_in_point)
- stewart_base_point = self._tf_listener.transformPoint('/stewart_base', temp_in_point)
+ transform = self._tf_buffer.lookup_transform('base_link', header.frame_id, rclpy.time.Time.from_msg(header.stamp))
+ base_link_point = tf2_geometry_msgs.do_transform_point(temp_in_point, transform)
point.x = base_link_point.point.x
point.y = base_link_point.point.y
- point.z = stewart_base_point.point.z - HEAD_ZERO_Z
+ point.z = base_link_point.point.z
def main():
- rospy.init_node('nearest_face_following_node')
- face_following_node = NearestFaceFollowingNode()
- face_following_node.run()
-
+ rclpy.init()
+ nearest_face_following_node = NearestFaceFollowingNode()
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ nearest_face_following_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ nearest_face_following_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/face_following/scripts/specific_face_following_node.py b/ros/behaviors/face_following/scripts/specific_face_following_node.py
index 11a324ff..0a64b9d2 100755
--- a/ros/behaviors/face_following/scripts/specific_face_following_node.py
+++ b/ros/behaviors/face_following/scripts/specific_face_following_node.py
@@ -1,10 +1,9 @@
#!/usr/bin/env python3
-import threading
+import rclpy
-import rospy
from std_msgs.msg import String
-from person_identification.msg import PersonNames
+from perception_msgs.msg import PersonNames
from t_top import vector_to_angles
@@ -13,33 +12,28 @@
class SpecificFaceFollowingNode(FaceFollowingNode):
def __init__(self):
- super().__init__(namespace='specific_face_following')
- self._direction_frame_id = rospy.get_param('~direction_frame_id')
+ super().__init__(node_name='specific_face_following_node', namespace='specific_face_following')
+ self._direction_frame_id = self.declare_parameter('direction_frame_id', 'odas').get_parameter_value().string_value
- self._target_name_lock = threading.Lock()
self._target_name = None
- self._target_name_sub = rospy.Subscriber('target_name', String, self._target_name_cb, queue_size=1)
- self._person_names_sub = rospy.Subscriber('person_names', PersonNames, self._person_names_cb, queue_size=1)
+ self._target_name_sub = self.create_subscription(String, 'target_name', self._target_name_cb, 1)
+ self._person_names_sub = self.create_subscription(PersonNames, 'person_names', self._person_names_cb, 1)
def _target_name_cb(self, msg):
- with self._target_name_lock:
- self._target_name = msg.data
+ self._target_name = msg.data
def _person_names_cb(self, msg):
if self._movement_commands.is_filtering_all_messages:
return
- with self._target_name_lock:
- target_name = self._target_name
-
for person_name in msg.names:
- if person_name.detection_type != 'voice' and person_name.name == target_name:
+ if person_name.detection_type != 'voice' and person_name.name == self._target_name:
if person_name.frame_id != self._direction_frame_id:
- rospy.logerr(f'Invalid direction frame id ({person_name.frame_id} != {self._direction_frame_id})')
+ self.get_logger().error(f'Invalid direction frame id ({person_name.frame_id} != {self._direction_frame_id})')
return
if len(person_name.direction) == 0 or len(person_name.position_2d) == 0:
- rospy.logerr(f'The direction and position_2d must not be empty.')
+ self.get_logger().error(f'The direction and position_2d must not be empty.')
return
yaw, _ = vector_to_angles(person_name.direction[0])
@@ -49,13 +43,18 @@ def _person_names_cb(self, msg):
def main():
- rospy.init_node('specific_face_following_node')
- face_following_node = SpecificFaceFollowingNode()
- face_following_node.run()
-
+ rclpy.init()
+ specific_face_following_node = SpecificFaceFollowingNode()
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ specific_face_following_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ specific_face_following_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/face_following/setup.py b/ros/behaviors/face_following/setup.py
deleted file mode 100644
index d3b27af4..00000000
--- a/ros/behaviors/face_following/setup.py
+++ /dev/null
@@ -1,12 +0,0 @@
-## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from setuptools import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
- packages=['face_following'],
- package_dir={'': 'src'},
-)
-
-setup(**setup_args)
diff --git a/ros/behaviors/face_following/src/face_following/lib_face_following_node.py b/ros/behaviors/face_following/src/face_following/lib_face_following_node.py
deleted file mode 100644
index 8e971edc..00000000
--- a/ros/behaviors/face_following/src/face_following/lib_face_following_node.py
+++ /dev/null
@@ -1,69 +0,0 @@
-import math
-import threading
-
-import rospy
-
-from t_top import MovementCommands, HEAD_ZERO_Z, HEAD_POSE_PITCH_INDEX
-
-
-TARGET_HEAD_IMAGE_Y = 0.5
-
-
-class FaceFollowingNode:
- def __init__(self, namespace):
- self._simulation = rospy.get_param('~simulation')
- self._rate = rospy.Rate(rospy.get_param('~control_frequency'))
- self._torso_control_alpha = rospy.get_param('~torso_control_alpha')
- self._head_control_p_gain = rospy.get_param('~head_control_p_gain')
- self._head_enabled = rospy.get_param('~head_enabled')
- self._min_head_pitch = rospy.get_param('~min_head_pitch_rad')
- self._max_head_pitch = rospy.get_param('~max_head_pitch_rad')
-
- self._target_lock = threading.Lock()
- self._target_torso_yaw = None
- self._current_head_image_y = None
-
- self._movement_commands = MovementCommands(self._simulation, namespace)
-
- def _update(self, yaw, head_image_y):
- with self._target_lock:
- if yaw is None or math.isfinite(yaw):
- self._target_torso_yaw = yaw
- if head_image_y is None or math.isfinite(head_image_y):
- self._current_head_image_y = head_image_y
-
- def run(self):
- while not rospy.is_shutdown():
- self._rate.sleep()
- if self._movement_commands.is_filtering_all_messages:
- continue
-
- self._update_torso()
- if self._head_enabled:
- self._update_head()
-
- def _update_torso(self):
- with self._target_lock:
- target_torso_yaw = self._target_torso_yaw
- if target_torso_yaw is None:
- return
-
- distance = target_torso_yaw - self._movement_commands.current_torso_pose
- if distance < -math.pi:
- distance = 2 * math.pi + distance
- elif distance > math.pi:
- distance = -(2 * math.pi - distance)
-
- pose = self._movement_commands.current_torso_pose + self._torso_control_alpha * distance
- self._movement_commands.move_torso(pose)
-
- def _update_head(self):
- with self._target_lock:
- current_head_image_y = self._current_head_image_y
- if current_head_image_y is None:
- return
-
- current_pitch = self._movement_commands.current_head_pose[HEAD_POSE_PITCH_INDEX]
- pitch = current_pitch + self._head_control_p_gain * (current_head_image_y - TARGET_HEAD_IMAGE_Y)
- pitch = max(self._min_head_pitch, min(pitch, self._max_head_pitch))
- self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, pitch, 0])
diff --git a/ros/behaviors/gesture/CMakeLists.txt b/ros/behaviors/gesture/CMakeLists.txt
index 5efe4fca..c64c1411 100644
--- a/ros/behaviors/gesture/CMakeLists.txt
+++ b/ros/behaviors/gesture/CMakeLists.txt
@@ -1,205 +1,33 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(gesture)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## 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
- hbba_lite
- rospy
- std_msgs
- message_generation
- t_top
- )
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
- GestureName.msg
- Done.msg
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(daemon_ros_client REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(t_top REQUIRED)
+find_package(behavior_msgs REQUIRED)
+
+# Python Nodes
+install(PROGRAMS
+ scripts/gesture_node.py
+ DESTINATION lib/${PROJECT_NAME}
)
-## 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()
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 gesture
- # CATKIN_DEPENDS hbba_lite rospy std_msgs t_top
- # DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
- # include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/gesture.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/gesture_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-catkin_install_python(PROGRAMS
- scripts/gesture_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_gesture.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_package()
diff --git a/ros/behaviors/gesture/README.md b/ros/behaviors/gesture/README.md
index 831a1810..dc25624a 100644
--- a/ros/behaviors/gesture/README.md
+++ b/ros/behaviors/gesture/README.md
@@ -10,23 +10,23 @@ This node makes T-Top perform gestures.
#### Parameters
-- `simulation` (bool): Indicates if it's used in the simulation.
+- `simulation` (bool): Indicates if it's used in the simulation. The default value is false.
#### Subscribed Topics
- `daemon/motor_status` ([daemon_ros_client/MotorStatus](../../daemon_ros_client/msg/MotorStatus.msg)): The motor status.
-- `gesture/name` ([gesture/GestureName](msg/GestureName.msg)): The gesture to perform.
+- `gesture/name` ([behavior_msgs/GestureName](../behavior_msgs/msg/GestureName.msg)): The gesture to perform.
- Supported values : `yes`, `no`, `maybe`, `origin_all`, `origin_head`, `origin_torso`
#### Published Topics
-- `gesture/set_head_pose` ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)):
+- `gesture/set_head_pose` ([geometry_msgs/PoseStamped](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/PoseStamped.html)):
The head pose.
-- `gesture/set_torso_orientation` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)): The
+- `gesture/set_torso_orientation` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)): The
torso orientation.
-- `gesture/done` ([gesture/Done](msg/Done.msg)): Indicates that the gesture is finished.
+- `gesture/done` ([behavior_msgs/Done](../behavior_msgs/msg/Done.msg)): Indicates that the gesture is finished.
#### Services
-- `pose/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `pose/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
diff --git a/ros/behaviors/gesture/package.xml b/ros/behaviors/gesture/package.xml
index 8db55954..9a3ffc9e 100644
--- a/ros/behaviors/gesture/package.xml
+++ b/ros/behaviors/gesture/package.xml
@@ -1,74 +1,29 @@
-
+
+
gesture
0.0.0
The gesture package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclpy
+ daemon_ros_client
+ std_msgs
+ hbba_lite
+ t_top
+ behavior_msgs
-
-
-
- TODO
+ rosidl_default_runtime
+ ros2launch
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- hbba_lite
- rospy
- std_msgs
- t_top
- message_generation
- hbba_lite
- rospy
- std_msgs
- t_top
- message_generation
- hbba_lite
- rospy
- std_msgs
- t_top
- message_generation
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/gesture/scripts/gesture_node.py b/ros/behaviors/gesture/scripts/gesture_node.py
index 8b1902a6..2a46d322 100755
--- a/ros/behaviors/gesture/scripts/gesture_node.py
+++ b/ros/behaviors/gesture/scripts/gesture_node.py
@@ -1,9 +1,10 @@
#!/usr/bin/env python3
-import threading
+import rclpy
+import rclpy.node
+import rclpy.executors
-import rospy
-from gesture.msg import GestureName, Done
+from behavior_msgs.msg import GestureName, Done
from t_top import MovementCommands
@@ -17,28 +18,27 @@
MOVE_SAD_TIMEOUT = 5
-class GestureNode:
+class GestureNode(rclpy.node.Node):
def __init__(self):
- self._simulation = rospy.get_param('~simulation')
+ super().__init__('gesture_node')
- self._gesture_lock = threading.Lock()
- self._movement_commands = MovementCommands(self._simulation, namespace='gesture')
+ self._simulation = self.declare_parameter('simulation', False).get_parameter_value().bool_value
+ self._movement_commands = MovementCommands(self, self._simulation, namespace='gesture')
- self._done_pub = rospy.Publisher('gesture/done', Done, queue_size=5)
- self._gesture_sub = rospy.Subscriber('gesture/name', GestureName, self._on_gesture_cb)
+ self._done_pub = self.create_publisher(Done, 'gesture/done', 5)
+ self._gesture_sub = self.create_subscription(GestureName, 'gesture/name', self._on_gesture_cb, 5)
def _on_gesture_cb(self, msg):
- with self._gesture_lock:
- if self._movement_commands.is_filtering_all_messages:
- return
+ if self._movement_commands.is_filtering_all_messages:
+ return
- try:
- ok = self._execute_gesture(msg.name)
- except TimeoutError:
- rospy.logerr(f'The {msg.name} gesture has timed out.')
- ok = False
+ try:
+ ok = self._execute_gesture(msg.name)
+ except TimeoutError:
+ self.get_logger().error(f'The {msg.name} gesture has timed out.')
+ ok = False
- self._done_pub.publish(Done(id=msg.id, ok=ok))
+ self._done_pub.publish(Done(id=msg.id, ok=ok))
def _execute_gesture(self, name):
if name == 'yes':
@@ -61,23 +61,30 @@ def _execute_gesture(self, name):
elif name == 'sad':
self._movement_commands.move_head_to_sad(timeout=MOVE_SAD_TIMEOUT)
else:
- rospy.logerr(f'Invalid gesture name ({name})')
+ self.get_logger().error(f'Invalid gesture name ({name})')
return False
return True
def run(self):
- rospy.spin()
+ executor = rclpy.executors.MultiThreadedExecutor(num_threads=2)
+ executor.add_node(self)
+ executor.spin()
def main():
- rospy.init_node('gesture_node')
+ rclpy.init()
gesture_node = GestureNode()
- gesture_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ gesture_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ gesture_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/led_animations/CMakeLists.txt b/ros/behaviors/led_animations/CMakeLists.txt
index ef6f5494..51b9bd2b 100644
--- a/ros/behaviors/led_animations/CMakeLists.txt
+++ b/ros/behaviors/led_animations/CMakeLists.txt
@@ -1,211 +1,36 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(led_animations)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## 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
- daemon_ros_client
- hbba_lite
- message_generation
- rospy
- geometry_msgs
- sensor_msgs
-)
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
- Animation.msg
- Done.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
- daemon_ros_client
- geometry_msgs
- sensor_msgs
-)
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 led_animations
-# CATKIN_DEPENDS daemon_ros_client hbba_lite message_generation rospy
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/led_animations.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/led_animations_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-catkin_install_python(PROGRAMS
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(daemon_ros_client REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(t_top REQUIRED)
+find_package(behavior_msgs REQUIRED)
+
+# Python Librairies
+ament_python_install_package(${PROJECT_NAME})
+
+# Python Nodes
+install(PROGRAMS
scripts/led_animations_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ DESTINATION lib/${PROJECT_NAME}
)
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_led_animations.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_package()
diff --git a/ros/behaviors/led_animations/README.md b/ros/behaviors/led_animations/README.md
index 37f56cd1..156a57f4 100644
--- a/ros/behaviors/led_animations/README.md
+++ b/ros/behaviors/led_animations/README.md
@@ -8,16 +8,19 @@ This folder contains the node to perform animations with the LED strip.
This node performs animations with the LED strip.
+#### Parameters
+- `period_s` (double): The period in seconds of the animation timer. The default value is 0.0333.
+
#### Subscribed Topics
-- `led_animations/name` ([led_animations/Animation](msg/Animation.msg)): The animation parameters.
+- `led_animations/name` ([behavior_msgs/Animation](../behavior_msgs/msg/Animation.msg)): The animation parameters.
#### Published Topics
- `led_animations/set_led_colors` ([daemon_ros_client/LedColors](../../daemon_ros_client/msg/LedColors.msg)): The LED colors.
-- `led_animations/done` ([led_animations/Done](msg/Done.msg)): Indicates that the animation is finished.
+- `led_animations/done` ([led_animations/Done](../behavior_msgs/msg/Done.msg)): Indicates that the animation is finished.
#### Services
-- `set_led_colors/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `set_led_colors/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
diff --git a/ros/behaviors/talk/src/talk/__init__.py b/ros/behaviors/led_animations/led_animations/__init__.py
similarity index 100%
rename from ros/behaviors/talk/src/talk/__init__.py
rename to ros/behaviors/led_animations/led_animations/__init__.py
diff --git a/ros/behaviors/led_animations/src/led_animations/lib_led_animations.py b/ros/behaviors/led_animations/led_animations/lib_led_animations.py
similarity index 99%
rename from ros/behaviors/led_animations/src/led_animations/lib_led_animations.py
rename to ros/behaviors/led_animations/led_animations/lib_led_animations.py
index e2b91f6f..164f7278 100644
--- a/ros/behaviors/led_animations/src/led_animations/lib_led_animations.py
+++ b/ros/behaviors/led_animations/led_animations/lib_led_animations.py
@@ -1,6 +1,5 @@
from abc import ABC, abstractmethod
import random
-from typing import List
import numpy as np
diff --git a/ros/behaviors/led_animations/msg/Done.msg b/ros/behaviors/led_animations/msg/Done.msg
deleted file mode 100644
index dae3f6dd..00000000
--- a/ros/behaviors/led_animations/msg/Done.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-uint64 id
-bool ok
diff --git a/ros/behaviors/led_animations/package.xml b/ros/behaviors/led_animations/package.xml
index 0f370122..07d6b83c 100644
--- a/ros/behaviors/led_animations/package.xml
+++ b/ros/behaviors/led_animations/package.xml
@@ -1,75 +1,29 @@
-
+
+
led_animations
0.0.0
The led_animations package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclpy
+ daemon_ros_client
+ std_msgs
+ hbba_lite
+ t_top
+ behavior_msgs
-
-
-
- TODO
+ rosidl_default_runtime
+ ros2launch
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- daemon_ros_client
- hbba_lite
- message_generation
- rospy
- geometry_msgs
- sensor_msgs
- daemon_ros_client
- hbba_lite
- rospy
- geometry_msgs
- sensor_msgs
- daemon_ros_client
- hbba_lite
- rospy
- geometry_msgs
- sensor_msgs
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/led_animations/scripts/led_animations_node.py b/ros/behaviors/led_animations/scripts/led_animations_node.py
old mode 100644
new mode 100755
index 1e662fbd..61eb0be2
--- a/ros/behaviors/led_animations/scripts/led_animations_node.py
+++ b/ros/behaviors/led_animations/scripts/led_animations_node.py
@@ -1,10 +1,14 @@
-import json
+#!/usr/bin/env python3
+
+import traceback
import math
-import rospy
+import rclpy
+import rclpy.node
+from rclpy.duration import Duration
from daemon_ros_client.msg import LedColors
-from led_animations.msg import Animation, Done
+from behavior_msgs.msg import LedAnimation as LedAnimationMsg, Done
import hbba_lite
@@ -18,22 +22,25 @@
led_color.blue = 0
-class LedAnimationsNode:
+class LedAnimationsNode(rclpy.node.Node):
def __init__(self):
- self._period_s = rospy.get_param('~period_s', 0.0333)
+ super().__init__('led_animations_node')
+
+ self._period_s = self.declare_parameter('period_s', 0.0333).get_parameter_value().double_value
self._timer = None
self._timer_msg_id = -1
- self._timer_start_time_s = 0.0
- self._timer_duration_s = 0.0
+ self._timer_start_time = None
+ self._timer_finite = None
+ self._timer_duration = None
self._timer_animation = None
- self._led_colors_pub = hbba_lite.OnOffHbbaPublisher('led_animations/set_led_colors', LedColors, queue_size=1,
+ self._led_colors_pub = hbba_lite.OnOffHbbaPublisher(self, LedColors, 'led_animations/set_led_colors', 1,
state_service_name='set_led_colors/filter_state')
self._led_colors_pub.on_filter_state_changing(self._hbba_filter_state_cb)
- self._done_pub = rospy.Publisher('led_animations/done', Done, queue_size=5)
- self._led_emotion_sub = rospy.Subscriber('led_animations/animation', Animation, self._animation_cb, queue_size=1)
+ self._done_pub = self.create_publisher(Done, 'led_animations/done', 5)
+ self._led_emotion_sub = self.create_subscription(LedAnimationMsg, 'led_animations/animation', self._animation_cb, 1)
def _hbba_filter_state_cb(self, publish_forced, previous_is_filtering_all_messages, new_is_filtering_all_messages):
if not previous_is_filtering_all_messages and new_is_filtering_all_messages:
@@ -46,49 +53,56 @@ def _animation_cb(self, msg):
try:
animation = LedAnimation.from_name(msg.name, self._period_s, msg.speed, msg.colors)
+ self._start_timer(msg.id, animation, msg.duration_s)
except Exception as e:
- rospy.logerr(f'Unable to instantiate the LED animation ({e})')
+ tb = traceback.format_exc()
+ self.get_logger().error(f'Unable to instantiate the LED animation ({e}): {tb}')
self._done_pub.publish(Done(id=msg.id, ok=False))
- self._start_timer(msg.id, animation, msg.duration_s)
def _stop_timer(self):
if self._timer is not None:
- self._timer.shutdown()
+ self.destroy_timer(self._timer)
self._timer = None
self._timer_msg_id = -1
- self._timer_start_time_s = 0.0
- self._timer_duration_s = 0.0
+ self._timer_start_time = None
+ self._timer_duration = None
self._timer_animation = None
def _start_timer(self, id, animation, duration_s):
self._stop_timer()
self._timer_msg_id = id
- self._timer_start_time_s = rospy.get_time()
- self._timer_duration_s = duration_s
+ self._timer_start_time = self.get_clock().now()
+ self._timer_finite = math.isfinite(duration_s)
+ self._timer_duration = Duration(seconds=duration_s) if self._timer_finite else None
self._timer_animation = animation
- self._timer = rospy.Timer(rospy.Duration(self._period_s), self._timer_cb)
+ self._timer = self.create_timer(self._period_s, self._timer_cb)
- def _timer_cb(self, timer_event):
- if rospy.get_time() - self._timer_start_time_s > self._timer_duration_s:
+ def _timer_cb(self):
+ if not self._timer_finite or self.get_clock().now() - self._timer_start_time < self._timer_duration:
+ self._led_colors_pub.publish(self._timer_animation.update())
+ else:
self._led_colors_pub.publish(NONE_LED_COLORS)
self._done_pub.publish(Done(id=self._timer_msg_id, ok=True))
self._stop_timer()
- else:
- self._led_colors_pub.publish(self._timer_animation.update())
def run(self):
- rospy.spin()
+ rclpy.spin(self)
def main():
- rospy.init_node('led_animations_node')
+ rclpy.init()
led_animations_node = LedAnimationsNode()
- led_animations_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ led_animations_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ led_animations_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/led_animations/setup.py b/ros/behaviors/led_animations/setup.py
deleted file mode 100644
index ccbcc699..00000000
--- a/ros/behaviors/led_animations/setup.py
+++ /dev/null
@@ -1,12 +0,0 @@
-## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from setuptools import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
- packages=['led_animations'],
- package_dir={'': 'src'},
-)
-
-setup(**setup_args)
diff --git a/ros/behaviors/led_emotions/CMakeLists.txt b/ros/behaviors/led_emotions/CMakeLists.txt
index 9d019e95..91fea45f 100644
--- a/ros/behaviors/led_emotions/CMakeLists.txt
+++ b/ros/behaviors/led_emotions/CMakeLists.txt
@@ -1,207 +1,39 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(led_emotions)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## 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
- daemon_ros_client
- hbba_lite
- rospy
- std_msgs
-)
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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 # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 led_emotions
-# CATKIN_DEPENDS daemon_ros_client hbba_lite rospy
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/led_emotions.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/led_emotions_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-catkin_install_python(PROGRAMS
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(daemon_ros_client REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(t_top REQUIRED)
+
+# Python Nodes
+install(PROGRAMS
scripts/led_emotions_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ DESTINATION lib/${PROJECT_NAME}
)
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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}
-# )
+# Install the pattern files
+install(DIRECTORY led_patterns DESTINATION share/${PROJECT_NAME})
-#############
-## Testing ##
-#############
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_led_emotions.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+# Launch files
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ament_package()
diff --git a/ros/behaviors/led_emotions/README.md b/ros/behaviors/led_emotions/README.md
index 177333c3..924a85eb 100644
--- a/ros/behaviors/led_emotions/README.md
+++ b/ros/behaviors/led_emotions/README.md
@@ -10,11 +10,12 @@ This node makes T-Top express emotions with it LED strip.
#### Parameters
+- `period_s` (double): The period in second of the animation timer. The default value is 0.0333.
- `led_patterns_file` (string): The file path containing the LED patterns
#### Subscribed Topics
-- `led_emotions/name` ([std_msgs/String](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/String.html)): The emotion names.
+- `led_emotions/name` ([std_msgs/String](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/String.html)): The emotion names.
#### Published Topics
@@ -22,5 +23,5 @@ This node makes T-Top express emotions with it LED strip.
#### Services
-- `set_led_colors/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `set_led_colors/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
diff --git a/ros/behaviors/led_emotions/launch/test_led_emotions.launch.xml b/ros/behaviors/led_emotions/launch/test_led_emotions.launch.xml
new file mode 100644
index 00000000..cbfd7317
--- /dev/null
+++ b/ros/behaviors/led_emotions/launch/test_led_emotions.launch.xml
@@ -0,0 +1,5 @@
+
+
+
+
+
diff --git a/ros/behaviors/led_emotions/led_patterns.json b/ros/behaviors/led_emotions/led_patterns/led_patterns.json
similarity index 100%
rename from ros/behaviors/led_emotions/led_patterns.json
rename to ros/behaviors/led_emotions/led_patterns/led_patterns.json
diff --git a/ros/behaviors/led_emotions/package.xml b/ros/behaviors/led_emotions/package.xml
index 2ba432bc..13016a47 100644
--- a/ros/behaviors/led_emotions/package.xml
+++ b/ros/behaviors/led_emotions/package.xml
@@ -1,71 +1,29 @@
-
+
+
led_emotions
0.0.0
The led_emotions package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rosidl_default_generators
+ rclpy
+ daemon_ros_client
+ std_msgs
+ hbba_lite
+ t_top
-
-
-
- TODO
+ rosidl_default_runtime
+ ros2launch
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- daemon_ros_client
- hbba_lite
- rospy
- std_msgs
- daemon_ros_client
- hbba_lite
- rospy
- std_msgs
- daemon_ros_client
- hbba_lite
- rospy
- std_msgs
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/led_emotions/scripts/led_emotions_node.py b/ros/behaviors/led_emotions/scripts/led_emotions_node.py
old mode 100644
new mode 100755
index 5250cd68..a66d6162
--- a/ros/behaviors/led_emotions/scripts/led_emotions_node.py
+++ b/ros/behaviors/led_emotions/scripts/led_emotions_node.py
@@ -1,7 +1,10 @@
+#!/usr/bin/env python3
+
import json
import math
-import rospy
+import rclpy
+import rclpy.node
from std_msgs.msg import String
from daemon_ros_client.msg import LedColors
@@ -25,21 +28,25 @@ def __init__(self, red, green, blue, period_s, ratio):
self.ratio = ratio
-class LedEmotionsNode:
+class LedEmotionsNode(rclpy.node.Node):
def __init__(self):
- self._period_s = rospy.get_param('~period_s', 0.0333)
- with open(rospy.get_param('~led_patterns_file'), 'r') as f:
+ super().__init__('led_emotions_node')
+
+ self._period_s = self.declare_parameter('period_s', 0.0333).get_parameter_value().double_value
+ led_patterns_file = self.declare_parameter('led_patterns_file', '').get_parameter_value().string_value
+
+ with open(led_patterns_file, 'r') as f:
self._led_patterns_by_emotion_name = self._load_led_patterns(json.load(f))
self._timer = None
self._timer_time_s = 0
self._timer_pattern = None
- self._led_colors_pub = hbba_lite.OnOffHbbaPublisher('led_emotions/set_led_colors', LedColors, queue_size=1,
+ self._led_colors_pub = hbba_lite.OnOffHbbaPublisher(self, LedColors, 'led_emotions/set_led_colors', 1,
state_service_name='set_led_colors/filter_state')
self._led_colors_pub.on_filter_state_changing(self._hbba_filter_state_cb)
- self._led_emotion_sub = rospy.Subscriber('led_emotions/name', String, self._emotion_cb, queue_size=1)
+ self._led_emotion_sub = self.create_subscription(String, 'led_emotions/name', self._emotion_cb, 1)
def _load_led_patterns(self, json_dict):
@@ -62,21 +69,22 @@ def _emotion_cb(self, msg):
return
if msg.data not in self._led_patterns_by_emotion_name:
- rospy.logerr(f'Invalid emotion name ({msg.data})')
- self._start_timer(self._led_patterns_by_emotion_name[msg.data])
+ self.get_logger().error(f'Invalid emotion name ({msg.data})')
+ else:
+ self._start_timer(self._led_patterns_by_emotion_name[msg.data])
def _stop_timer(self):
if self._timer is not None:
- self._timer.shutdown()
+ self.destroy_timer(self._timer)
self._timer = None
def _start_timer(self, pattern):
self._stop_timer()
self._timer_time_s = 0
self._timer_pattern = pattern
- self._timer = rospy.Timer(rospy.Duration(self._period_s), self._timer_cb)
+ self._timer = self.create_timer(self._period_s, self._timer_cb)
- def _timer_cb(self, timer_event):
+ def _timer_cb(self):
self._timer_time_s += self._period_s
if self._timer_time_s > self._timer_pattern.period_s:
self._timer_time_s -= self._timer_pattern.period_s
@@ -107,17 +115,22 @@ def _get_brightness(self, t, T, ratio):
return 0
def run(self):
- rospy.spin()
+ rclpy.spin(self)
def main():
- rospy.init_node('led_emotions_node')
+ rclpy.init()
led_emotions_node = LedEmotionsNode()
- led_emotions_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ led_emotions_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ led_emotions_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/piper_ros/CMakeLists.txt b/ros/behaviors/piper_ros/CMakeLists.txt
index 948762bd..6e493b0e 100644
--- a/ros/behaviors/piper_ros/CMakeLists.txt
+++ b/ros/behaviors/piper_ros/CMakeLists.txt
@@ -1,18 +1,20 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(piper_ros)
-## Compile as C++11, supported in ROS Kinetic and newer
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-## 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
- message_generation
- roscpp
- roslib
-)
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
## System dependencies are found with CMake's conventions
# External Dependencies
@@ -34,12 +36,11 @@ link_directories(${CMAKE_BINARY_DIR}/install/fmt/lib)
# ******* eSpeak NG dependency *******
-set(ESPEAK_NG_DIR "${CMAKE_BINARY_DIR}/install/espeak-ng")
ExternalProject_Add(
espeak_ng_external
PREFIX "${CMAKE_CURRENT_BINARY_DIR}/espeak-ng"
URL "https://github.com/rhasspy/espeak-ng/archive/refs/tags/2023.9.7-4.zip"
- CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH=${ESPEAK_NG_DIR}
+ CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_INSTALL_PREFIX}
CMAKE_ARGS -DUSE_ASYNC:BOOL=OFF
CMAKE_ARGS -DBUILD_SHARED_LIBS:BOOL=ON
CMAKE_ARGS -DUSE_MBROLA:BOOL=OFF
@@ -50,9 +51,9 @@ ExternalProject_Add(
CMAKE_ARGS -DEXTRA_cmn:BOOL=ON
CMAKE_ARGS -DEXTRA_ru:BOOL=ON
)
-include_directories(${CMAKE_BINARY_DIR}/install/espeak-ng/include)
-link_directories(${CMAKE_BINARY_DIR}/install/espeak-ng/lib)
-add_compile_definitions(ESPEAK_NG_DATA_PATH="${CMAKE_BINARY_DIR}/install/espeak-ng/share/espeak-ng-data")
+include_directories(${CMAKE_INSTALL_PREFIX}/include)
+link_directories(${CMAKE_INSTALL_PREFIX}/lib)
+add_compile_definitions(ESPEAK_NG_DATA_PATH="${CMAKE_INSTALL_PREFIX}/share/espeak-ng-data")
# ******* ONNX Runtime dependency *******
@@ -129,192 +130,23 @@ if (NOT EXISTS ${CMAKE_BINARY_DIR}/piper-${PIPER_TAG}/CMakeLists.txt)
endif()
include_directories(${CMAKE_BINARY_DIR}/piper-${PIPER_TAG}/src/cpp)
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
- GenerateSpeechFromText.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()
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 piper_ros
-# CATKIN_DEPENDS message_generation roscpp
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/piper_ros.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(behavior_srvs REQUIRED)
+find_package(ament_index_cpp REQUIRED)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
+# C++ Nodes
add_executable(piper_node src/piper_node.cpp ${CMAKE_BINARY_DIR}/piper-${PIPER_TAG}/src/cpp/piper.cpp)
-add_dependencies(piper_node fmt_external)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(piper_node
- ${catkin_LIBRARIES}
- piper_phonemize
- fmt
- spdlog
- ${ONNXRUNTIME_LIBRARIES}
-)
-
-#############
-## 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
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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 ##
-#############
+ament_target_dependencies(piper_node rclcpp behavior_srvs ament_index_cpp)
+target_link_libraries(piper_node piper_phonemize fmt spdlog ${ONNXRUNTIME_LIBRARIES})
+install(TARGETS piper_node DESTINATION lib/${PROJECT_NAME})
+add_dependencies(piper_node fmt_external espeak_ng_external)
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_piper_ros.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+# Install models
+install(DIRECTORY models DESTINATION share/${PROJECT_NAME})
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ament_package()
diff --git a/ros/behaviors/piper_ros/README.md b/ros/behaviors/piper_ros/README.md
index 126e0a0d..69f0a8d2 100644
--- a/ros/behaviors/piper_ros/README.md
+++ b/ros/behaviors/piper_ros/README.md
@@ -11,9 +11,9 @@ This node generates speech files with [Piper](https://github.com/rhasspy/piper).
#### Parameters
-- `use_gpu_if_available` (bool): Indicates whether to use the GPU or not.
+- `use_gpu_if_available` (bool): Indicates whether to use the GPU or not. The default value is false.
#### Services
-- `piper/generate_speech_from_text` ([piper_ros/GenerateSpeechFromText](srv/GenerateSpeechFromText.srv)):
+- `piper/generate_speech_from_text` ([behavior_srvs/GenerateSpeechFromText](../behavior_srvs/srv/GenerateSpeechFromText.srv)):
The service to generate speech files.
diff --git a/ros/behaviors/piper_ros/package.xml b/ros/behaviors/piper_ros/package.xml
index 0bd92bfe..476152c5 100644
--- a/ros/behaviors/piper_ros/package.xml
+++ b/ros/behaviors/piper_ros/package.xml
@@ -1,63 +1,26 @@
-
+
+
piper_ros
0.0.0
The piper_ros package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclcpp
+ ament_index_cpp
+ behavior_srvs
-
-
-
- TODO
+ rosidl_default_runtime
+ ros2launch
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- message_generation
- roscpp
- roscpp
- roscpp
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/piper_ros/src/piper_node.cpp b/ros/behaviors/piper_ros/src/piper_node.cpp
index 7c0ca43a..882163cb 100644
--- a/ros/behaviors/piper_ros/src/piper_node.cpp
+++ b/ros/behaviors/piper_ros/src/piper_node.cpp
@@ -1,7 +1,8 @@
-#include
+#include
-#include
-#include
+#include
+
+#include
#include
@@ -53,12 +54,11 @@ std::optional genderFromString(const std::string& str)
}
-class PiperNode
+class PiperNode : public rclcpp::Node
{
bool m_useGpuIfAvailable;
- ros::NodeHandle m_nodeHandle;
- ros::ServiceServer m_generateSpeechFromTextService;
+ rclcpp::Service::SharedPtr m_generateSpeechFromTextService;
piper::PiperConfig m_piperConfig;
piper::Voice m_englishFemaleVoice;
@@ -67,8 +67,10 @@ class PiperNode
piper::Voice m_frenchMaleVoice;
public:
- explicit PiperNode(bool useGpuIfAvailable) : m_useGpuIfAvailable(useGpuIfAvailable)
+ explicit PiperNode() : rclcpp::Node("piper_node")
{
+ m_useGpuIfAvailable = declare_parameter("use_gpu_if_available", false);
+
m_piperConfig.useESpeak = true;
m_piperConfig.eSpeakDataPath = ESPEAK_NG_DATA_PATH;
@@ -79,19 +81,22 @@ class PiperNode
piper::initialize(m_piperConfig);
-
- m_generateSpeechFromTextService = m_nodeHandle.advertiseService(
+ m_generateSpeechFromTextService = create_service(
"piper/generate_speech_from_text",
- &PiperNode::generateSpeechFromTextServiceCallback,
- this);
+ [this] (
+ const std::shared_ptr request,
+ std::shared_ptr response)
+ {
+ generateSpeechFromTextServiceCallback(request, response);
+ });
}
- void run() { ros::spin(); }
+ void run() { rclcpp::spin(shared_from_this()); }
private:
piper::Voice loadVoice(const char* filename)
{
- std::string modelFolder = ros::package::getPath("piper_ros") + "/models/";
+ std::string modelFolder = ament_index_cpp::get_package_share_directory("piper_ros") + "/models/";
piper::Voice voice;
voice.session.options.SetInterOpNumThreads(1);
@@ -109,7 +114,7 @@ class PiperNode
#else
if (m_useGpuIfAvailable)
{
- ROS_WARN("CUDA is not supported.");
+ RCLCPP_WARN(get_logger(), "CUDA is not supported.");
}
#endif
@@ -124,30 +129,26 @@ class PiperNode
return voice;
}
- bool generateSpeechFromTextServiceCallback(
- piper_ros::GenerateSpeechFromText::Request& request,
- piper_ros::GenerateSpeechFromText::Response& response)
+ void generateSpeechFromTextServiceCallback(
+ const std::shared_ptr request,
+ std::shared_ptr response)
{
- std::optional language = languageFromString(request.language);
+ std::optional language = languageFromString(request->language);
if (language == std::nullopt)
{
- response.ok = false;
- response.message = "Invalid language";
- return true;
+ response->ok = false;
+ response->message = "Invalid language";
}
- std::optional gender = genderFromString(request.gender);
+ std::optional gender = genderFromString(request->gender);
if (gender == std::nullopt)
{
- response.ok = false;
- response.message = "Invalid gender";
- return true;
+ response->ok = false;
+ response->message = "Invalid gender";
}
- generateSpeechFromText(*language, *gender, request.length_scale, request.text, request.path);
- response.ok = true;
-
- return true;
+ generateSpeechFromText(*language, *gender, request->length_scale, request->text, request->path);
+ response->ok = true;
}
void generateSpeechFromText(
@@ -194,19 +195,12 @@ class PiperNode
int main(int argc, char** argv)
{
- ros::init(argc, argv, "piper_node");
+ rclcpp::init(argc, argv);
- ros::NodeHandle privateNodeHandle("~");
-
- bool useGpuIfAvailable;
- if (!privateNodeHandle.getParam("use_gpu_if_available", useGpuIfAvailable))
- {
- ROS_ERROR("The parameter use_gpu_if_available is required.");
- return -1;
- }
+ auto node = std::make_shared();
+ node->run();
- PiperNode node(useGpuIfAvailable);
- node.run();
+ rclcpp::shutdown();
return 0;
}
diff --git a/ros/behaviors/sound_following/CMakeLists.txt b/ros/behaviors/sound_following/CMakeLists.txt
index 5f51c0e9..e70d1150 100644
--- a/ros/behaviors/sound_following/CMakeLists.txt
+++ b/ros/behaviors/sound_following/CMakeLists.txt
@@ -1,207 +1,33 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(sound_following)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## 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
- hbba_lite
- rospy
- t_top
- odas_ros
- )
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
-# geometry_msgs# std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 sound_following
- # CATKIN_DEPENDS geometry_msgs hbba_lite rospy std_msgs t_top tf
- # DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
- # include
- ${catkin_INCLUDE_DIRS}
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(daemon_ros_client REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(t_top REQUIRED)
+find_package(odas_ros_msgs REQUIRED)
+
+# Python Nodes
+install(PROGRAMS
+ scripts/sound_following_node.py
+ DESTINATION lib/${PROJECT_NAME}
)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/sound_following.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/sound_following_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-catkin_install_python(PROGRAMS
- scripts/sound_following_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_sound_following.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_package()
diff --git a/ros/behaviors/sound_following/README.md b/ros/behaviors/sound_following/README.md
index fdd72eae..2935afb1 100644
--- a/ros/behaviors/sound_following/README.md
+++ b/ros/behaviors/sound_following/README.md
@@ -10,32 +10,32 @@ This node makes T-Top follow the loudest sound.
#### Parameters
-- `simulation` (bool): Indicates if it's used in the simulation.
-- `control_frequency` (double): The frequency at which the pose messages are sent.
-- `torso_control_alpha` (double): The low-pass filter parameter for the torso pose.
-- `head_control_alpha` (double): The low-pass filter parameter for the head pose.
-- `head_enabled` (bool): Indicates if the head will move.
-- `min_head_pitch_rad` (double): The minimum pitch angle in radian of the head.
-- `max_head_pitch_rad` (double): The maximum pitch angle in radian of the head.
-- `min_activity` (double): The minimum activity level to consider the sound source valid.
-- `min_valid_source_pitch_rad` (double): The minimum pitch angle in radian to consider the sound source valid.
-- `max_valid_source_pitch_rad` (double): The maximum pitch angle in radian to consider the sound source valid.
-- `direction_frame_id` (string): The audio analysis frame id.
+- `simulation` (bool): Indicates if it's used in the simulation. The default value is false.
+- `control_frequency` (double): The frequency at which the pose messages are sent. The default value is 30.0.
+- `torso_control_alpha` (double): The low-pass filter parameter for the torso pose. The default value is 0.2.
+- `head_control_alpha` (double): The low-pass filter parameter for the head pose. The default value is 0.2.
+- `head_enabled` (bool): Indicates if the head will move. The default value is false.
+- `min_head_pitch_rad` (double): The minimum pitch angle in radian of the head. The default value is -0.35.
+- `max_head_pitch_rad` (double): The maximum pitch angle in radian of the head. The default value is 0.35.
+- `min_activity` (double): The minimum activity level to consider the sound source valid. The default value is 0.1.
+- `min_valid_source_pitch_rad` (double): The minimum pitch angle in radian to consider the sound source valid. The default value is -1.4.
+- `max_valid_source_pitch_rad` (double): The maximum pitch angle in radian to consider the sound source valid. The default value is 1.4.
+- `direction_frame_id` (string): The audio analysis frame id. The default value is odas.
#### Subscribed Topics
- `daemon/motor_status` ([daemon_ros_client/MotorStatus](../../daemon_ros_client/msg/MotorStatus.msg)): The motor status.
-- `sst` ([odas_ros/OdasSstArrayStamped](https://github.com/introlab/odas_ros/blob/main/msg/OdasSstArrayStamped.msg)):
+- `sst` ([odas_ros_msgs/OdasSstArrayStamped](https://github.com/introlab/odas_ros/blob/ros2/odas_ros_msgs/msg/OdasSstArrayStamped.msg)):
The sound source tracking information.
#### Published Topics
-- `sound_following/set_head_pose` ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)):
+- `sound_following/set_head_pose` ([geometry_msgs/PoseStamped](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/PoseStamped.html)):
The head pose.
-- `sound_following/set_torso_orientation` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)): The
+- `sound_following/set_torso_orientation` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)): The
torso orientation.
#### Services
-- `pose/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `pose/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
diff --git a/ros/behaviors/sound_following/package.xml b/ros/behaviors/sound_following/package.xml
index 0c3a0ac3..ba74ab03 100644
--- a/ros/behaviors/sound_following/package.xml
+++ b/ros/behaviors/sound_following/package.xml
@@ -1,71 +1,28 @@
-
+
+
sound_following
0.0.0
The sound_following package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclpy
+ daemon_ros_client
+ std_msgs
+ hbba_lite
+ t_top
+ odas_ros_msgs
-
-
-
- TODO
+ rosidl_default_runtime
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- hbba_lite
- rospy
- t_top
- odas_ros
- hbba_lite
- rospy
- t_top
- odas_ros
- hbba_lite
- rospy
- t_top
- odas_ros
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/sound_following/scripts/sound_following_node.py b/ros/behaviors/sound_following/scripts/sound_following_node.py
index 8efb11c7..33fcc318 100755
--- a/ros/behaviors/sound_following/scripts/sound_following_node.py
+++ b/ros/behaviors/sound_following/scripts/sound_following_node.py
@@ -1,10 +1,12 @@
#!/usr/bin/env python3
import math
-import threading
-import rospy
-from odas_ros.msg import OdasSstArrayStamped
+import rclpy
+import rclpy.node
+import rclpy.executors
+
+from odas_ros_msgs.msg import OdasSstArrayStamped
from t_top import MovementCommands, vector_to_angles, HEAD_ZERO_Z, HEAD_POSE_PITCH_INDEX
@@ -12,35 +14,38 @@
TARGET_TOLERANCE = 0.02
-class SoundFollowingNode:
+class SoundFollowingNode(rclpy.node.Node):
def __init__(self):
- self._simulation = rospy.get_param('~simulation')
- self._rate = rospy.Rate(rospy.get_param('~control_frequency'))
- self._torso_control_alpha = rospy.get_param('~torso_control_alpha')
- self._head_control_alpha = rospy.get_param('~head_control_alpha')
- self._head_enabled = rospy.get_param('~head_enabled')
- self._min_head_pitch = rospy.get_param('~min_head_pitch_rad')
- self._max_head_pitch = rospy.get_param('~max_head_pitch_rad')
- self._min_activity = rospy.get_param('~min_activity')
- self._min_valid_source_pitch = rospy.get_param('~min_valid_source_pitch_rad')
- self._max_valid_source_pitch = rospy.get_param('~max_valid_source_pitch_rad')
- self._direction_frame_id = rospy.get_param('~direction_frame_id')
-
- self._target_lock = threading.Lock()
+ super().__init__('sound_following_node')
+
+ self._simulation = self.declare_parameter('simulation', False).get_parameter_value().bool_value
+ self._control_frequency = self.declare_parameter('control_frequency', 30.0).get_parameter_value().double_value
+ self._torso_control_alpha = self.declare_parameter('torso_control_alpha', 0.2).get_parameter_value().double_value
+ self._head_control_alpha = self.declare_parameter('head_control_alpha', 0.2).get_parameter_value().double_value
+ self._head_enabled = self.declare_parameter('head_enabled', False).get_parameter_value().bool_value
+ self._min_head_pitch = self.declare_parameter('min_head_pitch_rad', -0.35).get_parameter_value().double_value
+ self._max_head_pitch = self.declare_parameter('max_head_pitch_rad', 0.35).get_parameter_value().double_value
+ self._min_activity = self.declare_parameter('min_activity', 0.1).get_parameter_value().double_value
+ self._min_valid_source_pitch = self.declare_parameter('min_valid_source_pitch_rad', -1.4).get_parameter_value().double_value
+ self._max_valid_source_pitch = self.declare_parameter('max_valid_source_pitch_rad', 1.4).get_parameter_value().double_value
+ self._direction_frame_id = self.declare_parameter('direction_frame_id', 'odas').get_parameter_value().string_value
+
self._target_torso_yaw = None
self._target_head_pitch = None
- self._movement_commands = MovementCommands(self._simulation, namespace='sound_following')
- self._sst_sub = rospy.Subscriber('sst', OdasSstArrayStamped, self._sst_cb, queue_size=10)
+ self._movement_commands = MovementCommands(self, self._simulation, namespace='sound_following')
+ self._sst_sub = self.create_subscription(OdasSstArrayStamped, 'sst', self._sst_cb, 10)
+
+ self._timer = self.create_timer(1 / self._control_frequency, self._timer_callback)
def _sst_cb(self, sst):
if self._movement_commands.is_filtering_all_messages:
return
if len(sst.sources) > 1:
- rospy.logerr(f'Invalid sst (len(sst.sources)={len(sst.sources)})')
+ self.get_logger().error(f'Invalid sst (len(sst.sources)={len(sst.sources)})')
return
if sst.header.frame_id != self._direction_frame_id:
- rospy.logerr(f'Invalid direction frame id ({sst.header.frame_id} != {self._direction_frame_id})')
+ self.get_logger().error(f'Invalid direction frame id ({sst.header.frame_id} != {self._direction_frame_id})')
return
if len(sst.sources) == 0 or sst.sources[0].activity < self._min_activity:
return
@@ -49,27 +54,27 @@ def _sst_cb(self, sst):
if pitch < self._min_valid_source_pitch or pitch > self._max_valid_source_pitch:
return
- with self._target_lock:
- self._target_torso_yaw = yaw
- self._target_head_pitch = None if pitch is None else max(self._min_head_pitch, min(pitch, self._max_head_pitch))
+ self._target_torso_yaw = yaw
+ self._target_head_pitch = None if pitch is None else max(self._min_head_pitch, min(pitch, self._max_head_pitch))
- def run(self):
- while not rospy.is_shutdown():
- self._rate.sleep()
- if self._movement_commands.is_filtering_all_messages:
- continue
+ def _timer_callback(self):
+ if self._movement_commands.is_filtering_all_messages:
+ return
- self._update_torso()
- if self._head_enabled:
- self._update_head()
+ self._update_torso()
+ if self._head_enabled:
+ self._update_head()
+
+ def run(self):
+ executor = rclpy.executors.MultiThreadedExecutor(num_threads=2)
+ executor.add_node(self)
+ executor.spin()
def _update_torso(self):
- with self._target_lock:
- target_torso_yaw = self._target_torso_yaw
- if target_torso_yaw is None:
+ if self._target_torso_yaw is None:
return
- distance = target_torso_yaw - self._movement_commands.current_torso_pose
+ distance = self._target_torso_yaw - self._movement_commands.current_torso_pose
if abs(distance) < TARGET_TOLERANCE:
return
@@ -82,27 +87,30 @@ def _update_torso(self):
self._movement_commands.move_torso(pose)
def _update_head(self):
- with self._target_lock:
- target_head_pitch = self._target_head_pitch
- if target_head_pitch is None:
+ if self._target_head_pitch is None:
return
current_pitch = self._movement_commands.current_head_pose[HEAD_POSE_PITCH_INDEX]
- if abs(target_head_pitch - current_pitch) < TARGET_TOLERANCE:
+ if abs(self._target_head_pitch - current_pitch) < TARGET_TOLERANCE:
return
- pitch = self._head_control_alpha * target_head_pitch + (1 - self._head_control_alpha) * current_pitch
+ pitch = self._head_control_alpha * self._target_head_pitch + (1 - self._head_control_alpha) * current_pitch
self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, 0, pitch, 0])
def main():
- rospy.init_node('sound_following_node')
+ rclpy.init()
sound_following_node = SoundFollowingNode()
- sound_following_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ sound_following_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ sound_following_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/sound_object_person_following/.gitignore b/ros/behaviors/sound_object_person_following/.gitignore
deleted file mode 100644
index a2a4546e..00000000
--- a/ros/behaviors/sound_object_person_following/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-camera_3d_calibration.json
diff --git a/ros/behaviors/sound_object_person_following/CMakeLists.txt b/ros/behaviors/sound_object_person_following/CMakeLists.txt
index 183dc689..7646ef7e 100644
--- a/ros/behaviors/sound_object_person_following/CMakeLists.txt
+++ b/ros/behaviors/sound_object_person_following/CMakeLists.txt
@@ -1,215 +1,47 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(sound_object_person_following)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-## 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
- geometry_msgs
- hbba_lite
- odas_ros
- rospy
- std_msgs
- t_top
- tf
- video_analyzer
- message_filters
- sensor_msgs
- cv_bridge
-)
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
-# geometry_msgs# std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 sound_object_person_following
-# CATKIN_DEPENDS geometry_msgs hbba_lite odas_ros rospy std_msgs t_top tf video_analyzer
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/sound_object_person_following.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/sound_object_person_following_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-catkin_install_python(PROGRAMS
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(odas_ros_msgs REQUIRED)
+find_package(t_top REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(perception_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+
+# Python Librairies
+ament_python_install_package(${PROJECT_NAME})
+
+# Python Nodes
+install(PROGRAMS
scripts/calibrate_sound_object_person_following_node.py
scripts/sound_object_person_following_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ DESTINATION lib/${PROJECT_NAME}
)
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_sound_object_person_following.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+# Launch files
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ament_package()
diff --git a/ros/behaviors/sound_object_person_following/README.md b/ros/behaviors/sound_object_person_following/README.md
index 883d9c3d..0e2db10e 100644
--- a/ros/behaviors/sound_object_person_following/README.md
+++ b/ros/behaviors/sound_object_person_following/README.md
@@ -10,12 +10,12 @@ This node matches the 3d camera with the 2d wide camera.
#### Parameters
-- `match_count` (bool): Number of match to use.
+- `match_count` (bool): Number of match to use. The default value is 100.
#### Subscribed Topics
-- `camera_3d/color/image_raw` ([sensor_msgs/Image](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Image.html)): The rectified color image of the 3d camera.
-- `camera_2d_wide/image_rect` ([sensor_msgs/Image](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Image.html)): The rectified color image of the 2d wide camera.
+- `camera_3d/color/image_raw` ([sensor_msgs/Image](https://docs.ros.org/en/humble/p/sensor_msgs/interfaces/msg/Image.html)): The rectified color image of the 3d camera.
+- `camera_2d_wide/image_rect` ([sensor_msgs/Image](https://docs.ros.org/en/humble/p/sensor_msgs/interfaces/msg/Image.html)): The rectified color image of the 2d wide camera.
### `sound_object_person_following_node.py`
@@ -23,41 +23,40 @@ This node makes T-Top follow the loudest sound, the biggest (nearest) person and
#### Parameters
-- `simulation` (bool): Indicates if it's used in the simulation.
-- `control_frequency` (double): The frequency at which the pose messages are sent.
-- `torso_control_alpha` (double): The low-pass filter parameter for the torso pose (sound following).
-- `torso_control_p_gain` (double): The controller proportional gain for the torso pose.
-- `head_control_p_gain` (double): The controller proportional gain for the head pose.
-- `min_head_pitch_rad` (double): The minimum pitch angle in radian of the head.
-- `max_head_pitch_rad` (double): The maximum pitch angle in radian of the head.
-- `object_person_follower_type` (string): The object person follower type (bounding_box or semantic_segmentation).
+- `simulation` (bool): Indicates if it's used in the simulation. The default value is false.
+- `control_frequency` (double): The frequency at which the pose messages are sent. The default value is 30.
+- `torso_control_alpha` (double): The low-pass filter parameter for the torso pose (sound following). The default value is 0.2.
+- `torso_control_p_gain` (double): The controller proportional gain for the torso pose. The default value is 0.45.
+- `head_control_p_gain` (double): The controller proportional gain for the head pose. The default value is 0.45.
+- `min_head_pitch_rad` (double): The minimum pitch angle in radian of the head. The default value is -0.35.
+- `max_head_pitch_rad` (double): The maximum pitch angle in radian of the head. The default value is 0.35.
+- `object_person_follower_type` (string): The object person follower type (bounding_box or semantic_segmentation). The default value is bounding_box.
- `min_sst_activity` (double): The minimum activity level to consider the sound source valid.
- `min_valid_sst_pitch` (double): The minimum pitch angle in radian to consider the sound source valid.
- `max_valid_sst_pitch` (double): The maximum pitch angle in radian to consider the sound source valid.
- `direction_frame_id` (string): The audio analysis frame id.
-- `object_classes` (list of strings): The followed object classes. In launch files, use this syntax :
- `[]`.
+- `object_classes` (list of strings): The followed object classes.
- `padding` (double): The padding of the person and the objects.
- `target_lambda` (double): The loss scale factor centering the person.
#### Subscribed Topics
- `daemon/motor_status` ([daemon_ros_client/MotorStatus](../../daemon_ros_client/msg/MotorStatus.msg)): The motor status.
-- `sst` ([odas_ros/OdasSstArrayStamped](https://github.com/introlab/odas_ros/blob/main/msg/OdasSstArrayStamped.msg)):
+- `sst` ([odas_ros_msgs/OdasSstArrayStamped](https://github.com/introlab/odas_ros/blob/ros2/odas_ros_msgs/msg/OdasSstArrayStamped.msg)):
The sound source tracking information.
-- `video_analysis` ([video_analyzer/VideoAnalysis](../../perceptions/video_analyzer/msg/VideoAnalysis.msg)): The video
+- `video_analysis` ([perception_msgs/VideoAnalysis](../../perceptions/perception_msgs/msg/VideoAnalysis.msg)): The video
analysis containing the detected objects of the 2d wide camera.
#### Published Topics
-- `sound_object_person_following/set_head_pose` ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)):
+- `sound_object_person_following/set_head_pose` ([geometry_msgs/PoseStamped](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/PoseStamped.html)):
The head pose.
-- `sound_object_person_following/set_torso_orientation` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)):
+- `sound_object_person_following/set_torso_orientation` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)):
The torso orientation.
#### Services
-- `pose/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `pose/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
diff --git a/ros/behaviors/sound_object_person_following/launch/calibrate_sound_object_person_following.launch b/ros/behaviors/sound_object_person_following/launch/calibrate_sound_object_person_following.launch.xml
similarity index 69%
rename from ros/behaviors/sound_object_person_following/launch/calibrate_sound_object_person_following.launch
rename to ros/behaviors/sound_object_person_following/launch/calibrate_sound_object_person_following.launch.xml
index 07f514a5..bced4051 100644
--- a/ros/behaviors/sound_object_person_following/launch/calibrate_sound_object_person_following.launch
+++ b/ros/behaviors/sound_object_person_following/launch/calibrate_sound_object_person_following.launch.xml
@@ -1,10 +1,10 @@
-
+
-
+
diff --git a/ros/behaviors/sound_object_person_following/package.xml b/ros/behaviors/sound_object_person_following/package.xml
index 182ffdb8..b2973a20 100644
--- a/ros/behaviors/sound_object_person_following/package.xml
+++ b/ros/behaviors/sound_object_person_following/package.xml
@@ -1,92 +1,34 @@
-
+
+
sound_object_person_following
0.0.0
The sound_object_person_following package
+ Marc-Antoine Maheux
+ GPL-3.0 license
+
+ ament_cmake
+
+ rclpy
+ std_msgs
+ geometry_msgs
+ hbba_lite
+ odas_ros_msgs
+ t_top
+ tf2
+ tf2_ros
+ message_filters
+ perception_msgs
+ sensor_msgs
+ cv_bridge
+
+ rosidl_default_runtime
+ rosidl_interface_packages
+
+ ament_lint_auto
+ ament_lint_common
-
-
-
- marc-antoine
-
-
-
-
-
- TODO
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- geometry_msgs
- hbba_lite
- odas_ros
- rospy
- std_msgs
- t_top
- tf
- video_analyzer
- message_filters
- sensor_msgs
- cv_bridge
- geometry_msgs
- hbba_lite
- odas_ros
- rospy
- std_msgs
- t_top
- tf
- video_analyzer
- message_filters
- sensor_msgs
- cv_bridge
- geometry_msgs
- hbba_lite
- odas_ros
- rospy
- std_msgs
- t_top
- tf
- video_analyzer
- message_filters
- sensor_msgs
- cv_bridge
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/sound_object_person_following/scripts/calibrate_sound_object_person_following_node.py b/ros/behaviors/sound_object_person_following/scripts/calibrate_sound_object_person_following_node.py
index 8b3002cd..0d5b2a65 100755
--- a/ros/behaviors/sound_object_person_following/scripts/calibrate_sound_object_person_following_node.py
+++ b/ros/behaviors/sound_object_person_following/scripts/calibrate_sound_object_person_following_node.py
@@ -1,12 +1,13 @@
#!/usr/bin/env python3
-import threading
import math
import numpy as np
import cv2
-import rospy
+import rclpy
+import rclpy.node
+
import message_filters
from cv_bridge import CvBridge
@@ -17,7 +18,6 @@
MIN_MATCH_COUNT = 5
LOWES_RATIO_THRESHOLD = 0.7
-INACTIVE_SLEEP_DURATION = 1.0
class Match:
@@ -32,9 +32,11 @@ def __init__(self, source_points, destination_points,
self.destination_height = destination_height
-class CalibrateSoundObjectPersonFollowingNode:
+class CalibrateSoundObjectPersonFollowingNode(rclpy.node.Node):
def __init__(self):
- self._match_count = rospy.get_param('~match_count')
+ super().__init__('calibrate_sound_object_person_following_node')
+
+ self._match_count = self.declare_parameter('match_count', 100).get_parameter_value().integer_value
if self._match_count < 1:
raise ValueError('match_count must be at least 1.')
@@ -42,19 +44,17 @@ def __init__(self):
self._orb = cv2.ORB_create()
self._matcher = cv2.BFMatcher()
- self._matches_lock = threading.Lock()
self._matches = []
- self._calibration_lock = threading.Lock()
try:
self._calibration = Camera3dCalibration.load_json_file()
except FileNotFoundError:
self._calibration = None
- self._camera_2d_wide_cropped_image_pub = rospy.Publisher('camera_2d_wide/cropped_image', Image, queue_size=1)
+ self._camera_2d_wide_cropped_image_pub = self.create_publisher(Image, 'camera_2d_wide/cropped_image', 1)
- camera_3d_image_sub = message_filters.Subscriber('camera_3d/color/image_raw', Image)
- camera_2d_wide_image_sub = message_filters.Subscriber('camera_2d_wide/image_rect', Image)
+ camera_3d_image_sub = message_filters.Subscriber(self, Image, 'camera_3d/color/image_raw')
+ camera_2d_wide_image_sub = message_filters.Subscriber(self, Image, 'camera_2d_wide/image_rect')
self._image_ts = message_filters.ApproximateTimeSynchronizer([camera_3d_image_sub, camera_2d_wide_image_sub], 10, 0.1)
self._image_ts.registerCallback(self._image_cb)
@@ -62,16 +62,12 @@ def _image_cb(self, camera_3d_image_msg, camera_2d_wide_image_msg):
camera_3d_image = self._cv_bridge.imgmsg_to_cv2(camera_3d_image_msg, 'bgr8')
camera_2d_wide_image = self._cv_bridge.imgmsg_to_cv2(camera_2d_wide_image_msg, 'bgr8')
- with self._calibration_lock:
- calibration = self._calibration
-
- if calibration is None:
+ if self._calibration is None:
match = self._match_images(camera_3d_image, camera_2d_wide_image)
if match is not None:
- with self._matches_lock:
- self._matches.append(match)
+ self._matches.append(match)
else:
- self._publish_camera_2d_wide_cropped_image(camera_2d_wide_image, calibration)
+ self._publish_camera_2d_wide_cropped_image(camera_2d_wide_image, self._calibration)
def _match_images(self, source_image, destination_image):
source_keypoints, source_descriptors = self._orb.detectAndCompute(source_image, None)
@@ -84,7 +80,7 @@ def _match_images(self, source_image, destination_image):
good_matches.append(m)
if len(good_matches) < MIN_MATCH_COUNT:
- rospy.logwarn(f'Not enough ORB feature matches (count={len(good_matches)})')
+ self.get_logger().warn(f'Not enough ORB feature matches (count={len(good_matches)})')
return None
else:
source_points = np.float32([source_keypoints[m.queryIdx].pt for m in good_matches]).reshape(-1,1,2)
@@ -106,18 +102,16 @@ def _publish_camera_2d_wide_cropped_image(self, image, calibration):
self._camera_2d_wide_cropped_image_pub.publish(self._cv_bridge.cv2_to_imgmsg(camera_2d_wide_image_cropped, 'bgr8'))
def run(self):
- while not rospy.is_shutdown():
- with self._matches_lock, self._calibration_lock:
- match_count = len(self._matches)
- has_calibration = self._calibration is not None
+ while rclpy.ok():
+ has_calibration = self._calibration is not None
- if match_count >= self._match_count and not has_calibration:
+ if len(self._matches) >= self._match_count and not has_calibration:
self._calibration = self._find_transform(self._matches)
else:
if not has_calibration:
- rospy.loginfo(f'Match count: {len(self._matches)}')
+ self.get_logger().info(f'Match count: {len(self._matches)}')
- rospy.sleep(INACTIVE_SLEEP_DURATION)
+ rclpy.spin_once(self)
def _find_transform(self, matches):
source_points = np.concatenate([x.source_points for x in self._matches])
@@ -136,8 +130,8 @@ def _find_transform(self, matches):
width = source_width / destination_width * scale
height = source_height / destination_height * scale
- rospy.loginfo('******* Results *******')
- rospy.loginfo(f'scale={scale}, center_x={center_x}, center_y={center_y}, width={width}, height={height}, rotation={rotation}')
+ self.get_logger().info('******* Results *******')
+ self.get_logger().info(f'scale={scale}, center_x={center_x}, center_y={center_y}, width={width}, height={height}, rotation={rotation}')
calibration = Camera3dCalibration(center_x, center_y, width, height)
calibration.save_json_file()
@@ -145,13 +139,18 @@ def _find_transform(self, matches):
def main():
- rospy.init_node('calibrate_sound_object_person_following_node')
- explore_node = CalibrateSoundObjectPersonFollowingNode()
- explore_node.run()
-
+ rclpy.init()
+ calibrate_sound_object_person_following_node = CalibrateSoundObjectPersonFollowingNode()
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ calibrate_sound_object_person_following_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ calibrate_sound_object_person_following_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/sound_object_person_following/scripts/sound_object_person_following_node.py b/ros/behaviors/sound_object_person_following/scripts/sound_object_person_following_node.py
index 393824b7..5952b0d7 100755
--- a/ros/behaviors/sound_object_person_following/scripts/sound_object_person_following_node.py
+++ b/ros/behaviors/sound_object_person_following/scripts/sound_object_person_following_node.py
@@ -2,14 +2,16 @@
from abc import ABC, abstractmethod
import math
-import threading
import numpy as np
from scipy.optimize import dual_annealing
-import rospy
-from odas_ros.msg import OdasSstArrayStamped
-from video_analyzer.msg import VideoAnalysis
+import rclpy
+import rclpy.node
+import rclpy.executors
+
+from odas_ros_msgs.msg import OdasSstArrayStamped
+from perception_msgs.msg import VideoAnalysis
from t_top import MovementCommands, vector_to_angles, HEAD_POSE_PITCH_INDEX, HEAD_ZERO_Z
@@ -43,27 +45,27 @@ def target(self):
class SoundFollower(Follower):
- def __init__(self, movement_commands):
+ def __init__(self, node, movement_commands):
+ self._node = node
self._movement_commands = movement_commands
- self._min_sst_activity = rospy.get_param('~min_sst_activity')
- self._min_valid_sst_pitch = rospy.get_param('~min_valid_sst_pitch')
- self._max_valid_sst_pitch = rospy.get_param('~max_valid_sst_pitch')
- self._direction_frame_id = rospy.get_param('~direction_frame_id')
+ self._min_sst_activity = self._node.declare_parameter('min_sst_activity', 0.1).get_parameter_value().double_value
+ self._min_valid_sst_pitch = self._node.declare_parameter('min_valid_sst_pitch', -1.4).get_parameter_value().double_value
+ self._max_valid_sst_pitch = self._node.declare_parameter('max_valid_sst_pitch', 1.4).get_parameter_value().double_value
+ self._direction_frame_id = self._node.declare_parameter('direction_frame_id', 'odas').get_parameter_value().string_value
- self._target_lock = threading.Lock()
self._target = None
- self._sst_sub = rospy.Subscriber('sst', OdasSstArrayStamped, self._sst_cb, queue_size=1)
+ self._sst_sub = self._node.create_subscription(OdasSstArrayStamped, 'sst', self._sst_cb, 1)
def _sst_cb(self, sst):
if self._movement_commands.is_filtering_all_messages:
return
if len(sst.sources) > 1:
- rospy.logerr(f'Invalid sst (len(sst.sources)={len(sst.sources)})')
+ self._node.get_logger().error(f'Invalid sst (len(sst.sources)={len(sst.sources)})')
return
if sst.header.frame_id != self._direction_frame_id:
- rospy.logerr(f'Invalid direction frame id ({sst.header.frame_id} != {self._direction_frame_id})')
+ self._node.get_logger().error(f'Invalid direction frame id ({sst.header.frame_id} != {self._direction_frame_id})')
return
if len(sst.sources) == 0 or sst.sources[0].activity < self._min_sst_activity:
return
@@ -72,29 +74,27 @@ def _sst_cb(self, sst):
if pitch < self._min_valid_sst_pitch or pitch > self._max_valid_sst_pitch:
return
- with self._target_lock:
- self._target = Target.from_sound_following(yaw)
+ self._target = Target.from_sound_following(yaw)
@property
def target(self):
- with self._target_lock:
- return self._target
+ return self._target
class ObjectPersonFollower(Follower):
- def __init__(self, camera_3d_calibration, movement_commands):
+ def __init__(self, node, camera_3d_calibration, movement_commands):
+ self._node = node
self._camera_3d_calibration = camera_3d_calibration
self._movement_commands = movement_commands
- self._object_classes = set(rospy.get_param('~object_classes'))
- self._padding = rospy.get_param('~padding')
- self._target_lambda = rospy.get_param('~target_lambda')
+ self._object_classes = set(self._node.declare_parameter('object_classes', ['all']).get_parameter_value().string_array_value)
+ self._padding = self._node.declare_parameter('padding', 0.075).get_parameter_value().double_value
+ self._target_lambda = self._node.declare_parameter('target_lambda', 0.005).get_parameter_value().double_value
_verify_padding(self._camera_3d_calibration, self._padding)
- self._target_lock = threading.Lock()
self._target = None
- self._video_analysis_sub = rospy.Subscriber('video_analysis', VideoAnalysis, self._video_analysis_cb, queue_size=1)
+ self._video_analysis_sub = self._node.create_subscription(VideoAnalysis, 'video_analysis', self._video_analysis_cb, 1)
def _video_analysis_cb(self, msg):
if self._movement_commands.is_filtering_all_messages:
@@ -106,8 +106,7 @@ def _video_analysis_cb(self, msg):
else:
target = self._find_target(msg, person)
- with self._target_lock:
- self._target = target
+ self._target = target
def _find_biggest_person(self, objects):
person_area_pairs = [(o, o.width_2d * o.height_2d) for o in objects if o.object_class == PERSON_CLASS]
@@ -141,8 +140,7 @@ def _find_target_range(self, person):
@property
def target(self):
- with self._target_lock:
- return self._target
+ return self._target
def _verify_padding(camera_3d_calibration, padding):
@@ -205,7 +203,7 @@ def _get_object_bounding_boxes(self, objects):
class SemanticSegmentationObjectPersonFollower(ObjectPersonFollower):
def _find_target(self, msg, person, eps=1e-6):
if len(msg.semantic_segmentation) == 0:
- rospy.logerr('The video analysis must have semantic segmentation.')
+ self._node.get_logger().error('The video analysis must have semantic segmentation.')
return
min_x, max_x, min_y, max_y = self._find_target_range(person)
@@ -246,39 +244,46 @@ def _get_mask_from_semantic_segmentation(self, semantic_segmentation):
return np.isin(class_indexes, desired_class_indexes)
-class SoundObjectPersonFollowingNode:
+class SoundObjectPersonFollowingNode(rclpy.node.Node):
def __init__(self):
- self._simulation = rospy.get_param('~simulation')
- self._rate = rospy.Rate(rospy.get_param('~control_frequency'))
- self._torso_control_alpha = rospy.get_param('~torso_control_alpha')
- self._torso_control_p_gain = rospy.get_param('~torso_control_p_gain')
- self._head_control_p_gain = rospy.get_param('~head_control_p_gain')
- self._min_head_pitch = rospy.get_param('~min_head_pitch_rad')
- self._max_head_pitch = rospy.get_param('~max_head_pitch_rad')
- self._object_person_follower_type = rospy.get_param('~object_person_follower_type')
+ super().__init__('sound_object_person_following_node')
+
+ self._simulation = self.declare_parameter('simulation', False).get_parameter_value().bool_value
+ self._control_frequency = self.declare_parameter('control_frequency', 30.0).get_parameter_value().double_value
+ self._torso_control_alpha = self.declare_parameter('torso_control_alpha', 0.2).get_parameter_value().double_value
+ self._torso_control_p_gain = self.declare_parameter('torso_control_p_gain', 0.45).get_parameter_value().double_value
+ self._head_control_p_gain = self.declare_parameter('head_control_p_gain', 0.45).get_parameter_value().double_value
+ self._min_head_pitch = self.declare_parameter('min_head_pitch_rad', -0.35).get_parameter_value().double_value
+ self._max_head_pitch = self.declare_parameter('max_head_pitch_rad', 0.35).get_parameter_value().double_value
+ self._object_person_follower_type = self.declare_parameter('object_person_follower_type', 'bounding_box').get_parameter_value().string_value
self._camera_3d_calibration = Camera3dCalibration.load_json_file()
- self._movement_commands = MovementCommands(self._simulation, namespace='sound_object_person_following')
- self._sound_follower = SoundFollower(self._movement_commands)
+ self._movement_commands = MovementCommands(self, self._simulation, namespace='sound_object_person_following')
+ self._sound_follower = SoundFollower(self, self._movement_commands)
if self._object_person_follower_type == 'bounding_box':
- self._object_person_follower = BoundingBoxObjectPersonFollower(self._camera_3d_calibration, self._movement_commands)
+ self._object_person_follower = BoundingBoxObjectPersonFollower(self, self._camera_3d_calibration, self._movement_commands)
elif self._object_person_follower_type == 'semantic_segmentation':
- self._object_person_follower = SemanticSegmentationObjectPersonFollower(self._camera_3d_calibration, self._movement_commands)
+ self._object_person_follower = SemanticSegmentationObjectPersonFollower(self, self._camera_3d_calibration, self._movement_commands)
else:
- raise ValueError(f'Invalid object_person_follower_type (object_person_follower_type={object_person_follower_type})')
+ raise ValueError(f'Invalid object_person_follower_type (object_person_follower_type={self._object_person_follower_type})')
- def run(self):
- while not rospy.is_shutdown():
- self._rate.sleep()
- if self._movement_commands.is_filtering_all_messages:
- continue
+ self._timer = self.create_timer(1 / self._control_frequency, self._timer_callback)
+
+ def _timer_callback(self):
+ if self._movement_commands.is_filtering_all_messages:
+ return
+
+ target = self._object_person_follower.target
+ if target is None:
+ target = self._sound_follower.target
- target = self._object_person_follower.target
- if target is None:
- target = self._sound_follower.target
+ if target is not None:
+ self._update_from_target(target)
- if target is not None:
- self._update_from_target(target)
+ def run(self):
+ executor = rclpy.executors.MultiThreadedExecutor(num_threads=2)
+ executor.add_node(self)
+ executor.spin()
def _update_from_target(self, target):
if target.yaw is not None:
@@ -312,13 +317,18 @@ def _update_head_from_image_y(self, current_image_y):
def main():
- rospy.init_node('sound_object_person_following_node')
- explore_node = SoundObjectPersonFollowingNode()
- explore_node.run()
-
+ rclpy.init()
+ sound_object_person_following_node = SoundObjectPersonFollowingNode()
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ sound_object_person_following_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ sound_object_person_following_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/sound_object_person_following/setup.py b/ros/behaviors/sound_object_person_following/setup.py
deleted file mode 100644
index 41aff439..00000000
--- a/ros/behaviors/sound_object_person_following/setup.py
+++ /dev/null
@@ -1,12 +0,0 @@
-## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from setuptools import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
- packages=['sound_object_person_following'],
- package_dir={'': 'src'},
-)
-
-setup(**setup_args)
diff --git a/ros/behaviors/sound_object_person_following/src/sound_object_person_following/__init__.py b/ros/behaviors/sound_object_person_following/sound_object_person_following/__init__.py
similarity index 81%
rename from ros/behaviors/sound_object_person_following/src/sound_object_person_following/__init__.py
rename to ros/behaviors/sound_object_person_following/sound_object_person_following/__init__.py
index b2878844..cad79278 100644
--- a/ros/behaviors/sound_object_person_following/src/sound_object_person_following/__init__.py
+++ b/ros/behaviors/sound_object_person_following/sound_object_person_following/__init__.py
@@ -1,11 +1,7 @@
import os
import json
-import rospkg
-
-
-PACKAGE_PATH = rospkg.RosPack().get_path('sound_object_person_following')
-CAMERA_3D_CALIBRATION_FILE_PATH = os.path.join(PACKAGE_PATH, 'camera_3d_calibration.json')
+CAMERA_3D_CALIBRATION_FILE_PATH = os.path.join(os.environ['HOME'], '.ros', 't-top', 'sound_object_person_following', 'camera_3d_calibration.json')
class Camera3dCalibration:
@@ -27,6 +23,7 @@ def load_json_file():
return Camera3dCalibration(data['center_x'], data['center_y'], data['width'], data['height'])
def save_json_file(self):
+ os.makedirs(os.path.dirname(CAMERA_3D_CALIBRATION_FILE_PATH), exist_ok=True)
with open(CAMERA_3D_CALIBRATION_FILE_PATH, 'w') as file:
data = {
'center_x': self.center_x,
diff --git a/ros/behaviors/sound_player/CMakeLists.txt b/ros/behaviors/sound_player/CMakeLists.txt
index 95e1fb4a..4154d67d 100644
--- a/ros/behaviors/sound_player/CMakeLists.txt
+++ b/ros/behaviors/sound_player/CMakeLists.txt
@@ -1,205 +1,31 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(sound_player)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## 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
- audio_utils
- message_generation
- rospy
- hbba_lite
- )
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
- SoundFile.msg
- Started.msg
- Done.msg
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(audio_utils_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(behavior_msgs REQUIRED)
+find_package(time_utils REQUIRED)
+
+# Python Nodes
+install(PROGRAMS
+ scripts/sound_player_node.py
+ DESTINATION lib/${PROJECT_NAME}
)
-## 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()
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 sound_player
- # CATKIN_DEPENDS audio_utils message_generation rospy
- # DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
- # include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/sound_player.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/sound_player_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_sound_player.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_package()
diff --git a/ros/behaviors/sound_player/README.md b/ros/behaviors/sound_player/README.md
index b336d21a..de776c82 100644
--- a/ros/behaviors/sound_player/README.md
+++ b/ros/behaviors/sound_player/README.md
@@ -10,20 +10,20 @@ This node plays sound files.
#### Parameters
-- `sampling_frequency` (int): The output sampling frequency.
-- `frame_sample_count` (double): The number of sample in each audio frame.
+- `sampling_frequency` (int): The output sampling frequency. The default value is 16000.
+- `frame_sample_count` (double): The number of sample in each audio frame. The default value is 1024.
#### Subscribed Topics
-- `sound_player/file` ([sound_player/SoundFile](msg/SoundFile.msg)): The sound file to play.
+- `sound_player/file` ([behavior_msgs/SoundFile](../behavior_msgs/msg/SoundFile.msg)): The sound file to play.
#### Published Topics
-- `audio_out` ([audio_utils/AudioFrame](https://github.com/introlab/audio_utils/blob/main/msg/AudioFrame.msg)): The
+- `audio_out` ([audio_utils_msgs/AudioFrame](https://github.com/introlab/audio_utils/blob/ros2/audio_utils_msgs/msg/AudioFrame.msg)): The
sound (mono, float format).
-- `sound_player/done` ([sound_player/Done](msg/Done.msg)): Indicates that the speech is finished.
+- `sound_player/done` ([behavior_msgs/Done](../behavior_msgs/msg/Done.msg)): Indicates that the speech is finished.
#### Services
-- `audio_out/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA
+- `audio_out/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA
filter state service to enable or disable the behavior.
diff --git a/ros/behaviors/sound_player/msg/Done.msg b/ros/behaviors/sound_player/msg/Done.msg
deleted file mode 100644
index dae3f6dd..00000000
--- a/ros/behaviors/sound_player/msg/Done.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-uint64 id
-bool ok
diff --git a/ros/behaviors/sound_player/package.xml b/ros/behaviors/sound_player/package.xml
index c223ce79..238a50bc 100644
--- a/ros/behaviors/sound_player/package.xml
+++ b/ros/behaviors/sound_player/package.xml
@@ -1,69 +1,28 @@
-
+
+
sound_player
0.0.0
The sound_player package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclpy
+ audio_utils_msgs
+ hbba_lite
+ behavior_msgs
+ time_utils
-
-
-
- TODO
+ rosidl_default_runtime
+ ros2launch
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- audio_utils
- message_generation
- rospy
- hbba_lite
- audio_utils
- rospy
- hbba_lite
- audio_utils
- rospy
- hbba_lite
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/sound_player/scripts/sound_player_node.py b/ros/behaviors/sound_player/scripts/sound_player_node.py
index 61b006ee..b61ba1b4 100755
--- a/ros/behaviors/sound_player/scripts/sound_player_node.py
+++ b/ros/behaviors/sound_player/scripts/sound_player_node.py
@@ -1,48 +1,52 @@
#!/usr/bin/env python3
-import threading
+import time
from pathlib import Path
import numpy as np
import librosa
-import rospy
-from sound_player.msg import SoundFile, Started, Done
-from audio_utils.msg import AudioFrame
+import rclpy
+import rclpy.node
+
+from behavior_msgs.msg import SoundFile, SoundStarted, Done
+from audio_utils_msgs.msg import AudioFrame
import hbba_lite
+import time_utils
-class SoundPlayerNode:
+class SoundPlayerNode(rclpy.node.Node):
def __init__(self):
- self._sampling_frequency = rospy.get_param('~sampling_frequency')
- self._frame_sample_count = rospy.get_param('~frame_sample_count')
+ super().__init__('sound_player_node')
+
+ self._sampling_frequency = self.declare_parameter('sampling_frequency', 16000).get_parameter_value().integer_value
+ self._frame_sample_count = self.declare_parameter('frame_sample_count', 1024).get_parameter_value().integer_value
- self._audio_pub = hbba_lite.OnOffHbbaPublisher('audio_out', AudioFrame, queue_size=5)
- self._started_pub = rospy.Publisher('sound_player/started', Started, queue_size=5)
- self._done_pub = rospy.Publisher('sound_player/done', Done, queue_size=5)
+ self._audio_pub = hbba_lite.OnOffHbbaPublisher(self, AudioFrame, 'audio_out', 5)
+ self._started_pub = self.create_publisher(SoundStarted, 'sound_player/started', 5)
+ self._done_pub = self.create_publisher(Done, 'sound_player/done', 5)
- self._file_sub_lock = threading.Lock()
- self._file_sub = rospy.Subscriber('sound_player/file', SoundFile, self._on_file_received_cb, queue_size=1)
+ self._file_sub = self.create_subscription(SoundFile, 'sound_player/file', self._on_file_received_cb, 1)
def _on_file_received_cb(self, msg):
- with self._file_sub_lock:
- if self._audio_pub.is_filtering_all_messages:
- return
+ if self._audio_pub.is_filtering_all_messages:
+ return
- try:
- self._play_audio(msg.id, msg.path)
- ok = True
- except Exception as e:
- rospy.logerr(f'Unable to play the sound ({e})')
- ok = False
- self._done_pub.publish(Done(id=msg.id, ok=ok))
+ try:
+ self._play_audio(msg.id, msg.path)
+ ok = True
+ except Exception as e:
+ self.get_logger().error(f'Unable to play the sound ({e})')
+ ok = False
+
+ self._done_pub.publish(Done(id=msg.id, ok=ok))
def _play_audio(self, id, path):
frames = self._load_frames(Path(path).expanduser().resolve())
- self._started_pub.publish(Started(id=id))
+ self._started_pub.publish(SoundStarted(id=id))
audio_frame = AudioFrame()
audio_frame.format = 'float'
@@ -50,12 +54,12 @@ def _play_audio(self, id, path):
audio_frame.sampling_frequency = self._sampling_frequency
audio_frame.frame_sample_count = self._frame_sample_count
- rate = rospy.Rate(self._sampling_frequency / self._frame_sample_count)
+ rate = time_utils.Rate(self._sampling_frequency / self._frame_sample_count)
for frame in frames:
if self._audio_pub.is_filtering_all_messages:
break
- audio_frame.header.stamp = rospy.Time.now()
+ audio_frame.header.stamp = self.get_clock().now().to_msg()
audio_frame.data = frame.tobytes()
self._audio_pub.publish(audio_frame)
@@ -63,23 +67,29 @@ def _play_audio(self, id, path):
def _load_frames(self, file_path):
waveform, _ = librosa.load(file_path, sr=self._sampling_frequency, res_type='kaiser_fast')
+ waveform = librosa.to_mono(waveform)
pad = (self._frame_sample_count - (waveform.shape[0] % self._frame_sample_count)) % self._frame_sample_count
waveform.resize(waveform.shape[0] + pad, refcheck=False)
frames = np.split(waveform, np.arange(self._frame_sample_count, len(waveform), self._frame_sample_count))
return frames
def run(self):
- rospy.spin()
+ rclpy.spin(self)
def main():
- rospy.init_node('sound_player_node')
+ rclpy.init()
sound_player_node = SoundPlayerNode()
- sound_player_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ sound_player_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ sound_player_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/talk/.gitignore b/ros/behaviors/talk/.gitignore
deleted file mode 100644
index 57293dd7..00000000
--- a/ros/behaviors/talk/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-audio_files/
diff --git a/ros/behaviors/talk/CMakeLists.txt b/ros/behaviors/talk/CMakeLists.txt
index e4dfaee0..7c0730c1 100644
--- a/ros/behaviors/talk/CMakeLists.txt
+++ b/ros/behaviors/talk/CMakeLists.txt
@@ -1,211 +1,35 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(talk)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## 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
- rospy
- std_msgs
- message_generation
- audio_utils
- hbba_lite
- piper_ros
- )
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
- Text.msg
- Done.msg
- Statistics.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
-)
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 talk
- CATKIN_DEPENDS roscpp rospy std_msgs
- # DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
- # include
- ${catkin_INCLUDE_DIRS}
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(audio_utils_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(behavior_srvs REQUIRED)
+find_package(behavior_msgs REQUIRED)
+find_package(time_utils REQUIRED)
+
+# Python Librairies
+ament_python_install_package(${PROJECT_NAME})
+
+# Python Nodes
+install(PROGRAMS
+ scripts/talk_node.py
+ DESTINATION lib/${PROJECT_NAME}
)
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/talk.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/talk_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-catkin_install_python(PROGRAMS
- scripts/talk_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_talk.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_package()
diff --git a/ros/behaviors/talk/README.md b/ros/behaviors/talk/README.md
index 29cf5e3c..c0f51275 100644
--- a/ros/behaviors/talk/README.md
+++ b/ros/behaviors/talk/README.md
@@ -10,28 +10,28 @@ This node makes T-Top talk and move its lips accordingly. It uses Google Cloud T
#### Parameters
-- `language` (string): The language (en or fr).
-- `gender` (string): The gender (female or male).
-- `speaking_rate` (doule): The speaking rate (range: [0.25, 4.0]).
-- `generator_type` (string): The generator type (google or piper). The Google generator uses the cloud and the Piper generator does not.
-- `mouth_signal_gain` (double): The gain to apply after the filter calculating the mouth signal.
-- `sampling_frequency` (int): The output sampling frequency.
-- `frame_sample_count` (double): The number of sample in each audio frame.
-- `done_delay_s` (double): The delay before sending the talk done message (default: 0.5).
+- `language` (string): The language (en or fr). The default value is en.
+- `gender` (string): The gender (female or male). The default value is male.
+- `speaking_rate` (doule): The speaking rate (range: [0.25, 4.0]). The default value is 1.0.
+- `generator_type` (string): The generator type (google or piper). The Google generator uses the cloud and the Piper generator does not. The default value is piper.
+- `mouth_signal_gain` (double): The gain to apply after the filter calculating the mouth signal. The default value is 0.04.
+- `sampling_frequency` (int): The output sampling frequency. The default value is 16000.
+- `frame_sample_count` (double): The number of sample in each audio frame. The default value is 1024.
+- `done_delay_s` (double): The delay before sending the talk done message. The default value is 0.5.
#### Subscribed Topics
-- `talk/text` ([talk/Text](msg/Text.msg)): The text to be said.
+- `talk/text` ([behavior_msgs/Text](../behavior_msgs/msg/Text.msg)): The text to be said.
#### Published Topics
-- `face/mouth_signal_scale` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)):
+- `face/mouth_signal_scale` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)):
Indicates how much the mouth must be open.
-- `audio_out` ([audio_utils/AudioFrame](https://github.com/introlab/audio_utils/blob/main/msg/AudioFrame.msg)): The
+- `audio_out` ([audio_utils_msgs/AudioFrame](https://github.com/introlab/audio_utils/blob/ros2/audio_utils_msgs/msg/AudioFrame.msg)): The
speech sound (mono, float format).
-- `talk/done` ([talk/Done](msg/Done.msg)): Indicates that the speech is finished.
+- `talk/done` ([behavior_msgs/Done](../behavior_msgs/msg/Done.msg)): Indicates that the speech is finished.
#### Services
-- `audio_out/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA
+- `audio_out/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA
filter state service to enable or disable the behavior.
diff --git a/ros/behaviors/talk/msg/Done.msg b/ros/behaviors/talk/msg/Done.msg
deleted file mode 100644
index dae3f6dd..00000000
--- a/ros/behaviors/talk/msg/Done.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-uint64 id
-bool ok
diff --git a/ros/behaviors/talk/package.xml b/ros/behaviors/talk/package.xml
index a18c5100..461c6713 100644
--- a/ros/behaviors/talk/package.xml
+++ b/ros/behaviors/talk/package.xml
@@ -1,80 +1,30 @@
-
+
+
talk
0.0.0
The talk package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- alexandre
+ ament_cmake
+ rclpy
+ std_msgs
+ audio_utils_msgs
+ hbba_lite
+ behavior_srvs
+ behavior_msgs
+ time_utils
-
-
-
- TODO
+ rosidl_default_runtime
+ ros2launch
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- roscpp
- rospy
- std_msgs
- audio_utils
- hbba_lite
- message_generation
- piper_ros
- roscpp
- rospy
- std_msgs
- audio_utils
- hbba_lite
- message_generation
- piper_ros
- roscpp
- rospy
- std_msgs
- audio_utils
- hbba_lite
- message_generation
- piper_ros
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/talk/scripts/talk_node.py b/ros/behaviors/talk/scripts/talk_node.py
index b6444590..b734b906 100755
--- a/ros/behaviors/talk/scripts/talk_node.py
+++ b/ros/behaviors/talk/scripts/talk_node.py
@@ -2,7 +2,7 @@
# -*- encoding: utf-8 -*-
import os
-import threading
+import time
from datetime import datetime
import numpy as np
@@ -10,83 +10,84 @@
import librosa
-import rospy
-import rospkg
+import rclpy
+import rclpy.node
+import rclpy.executors
+
from std_msgs.msg import Float32
-from talk.msg import Text, Done, Statistics
-from audio_utils.msg import AudioFrame
+from behavior_msgs.msg import Text, Done, Statistics
+from audio_utils_msgs.msg import AudioFrame
import hbba_lite
+import time_utils
from talk.lib_voice_generator import Language, Gender
from talk.lib_voice_generator import GoogleVoiceGenerator, PiperVoiceGenerator, CachedVoiceGenerator
-class TalkNode:
+class TalkNode(rclpy.node.Node):
def __init__(self):
- language = Language.from_name(rospy.get_param('~language'))
- gender = Gender.from_name(rospy.get_param('~gender'))
- speaking_rate = rospy.get_param('~speaking_rate')
- generator_type = rospy.get_param('~generator_type')
- cache_size = rospy.get_param('~cache_size')
+ super().__init__('talk_node')
+
+ language = Language.from_name(self.declare_parameter('language', 'en').get_parameter_value().string_value)
+ gender = Gender.from_name(self.declare_parameter('gender', 'male').get_parameter_value().string_value)
+ speaking_rate = self.declare_parameter('speaking_rate', 1.0).get_parameter_value().double_value
+ generator_type = self.declare_parameter('generator_type', 'piper').get_parameter_value().string_value
+ cache_size = self.declare_parameter('cache_size', 2000).get_parameter_value().integer_value
- self._mouth_signal_gain = rospy.get_param('~mouth_signal_gain')
- self._sampling_frequency = rospy.get_param('~sampling_frequency')
- self._frame_sample_count = rospy.get_param('~frame_sample_count')
- self._done_delay_s = rospy.get_param('~done_delay_s', 0.5)
+ self._mouth_signal_gain = self.declare_parameter('mouth_signal_gain', 0.04).get_parameter_value().double_value
+ self._sampling_frequency = self.declare_parameter('sampling_frequency', 16000).get_parameter_value().integer_value
+ self._frame_sample_count = self.declare_parameter('frame_sample_count', 1024).get_parameter_value().integer_value
+ self._done_delay_s = self.declare_parameter('done_delay_s', 0.5).get_parameter_value().double_value
- self._rospack = rospkg.RosPack()
- self._pkg_path = self._rospack.get_path('talk')
- audio_directory_path = os.path.join(self._pkg_path, 'audio_files')
+ audio_directory_path = os.path.join(os.environ['HOME'], '.ros', 't-top', 'talk', 'audio_files')
if generator_type == 'google':
self._voice_generator = GoogleVoiceGenerator(audio_directory_path, language, gender, speaking_rate)
elif generator_type == 'piper':
- self._voice_generator = PiperVoiceGenerator(audio_directory_path, language, gender, speaking_rate)
+ self._voice_generator = PiperVoiceGenerator(self, audio_directory_path, language, gender, speaking_rate)
else:
raise ValueError(f'Invalid generator type ({generator_type})')
if cache_size > 0:
self._voice_generator = CachedVoiceGenerator(self._voice_generator, cache_size)
- self._mouth_signal_scale_pub = rospy.Publisher('face/mouth_signal_scale', Float32, queue_size=5)
- self._audio_pub = hbba_lite.OnOffHbbaPublisher('audio_out', AudioFrame, queue_size=5)
- self._done_talking_pub = rospy.Publisher('talk/done', Done, queue_size=5)
- self._stats_pub = rospy.Publisher('talk/statistics', Statistics, queue_size=5)
+ self._mouth_signal_scale_pub = self.create_publisher(Float32, 'face/mouth_signal_scale', 5)
+ self._audio_pub = hbba_lite.OnOffHbbaPublisher(self, AudioFrame, 'audio_out', 5)
+ self._done_talking_pub = self.create_publisher(Done, 'talk/done', 5)
+ self._stats_pub = self.create_publisher(Statistics, 'talk/statistics', 5)
- self._text_sub_lock = threading.Lock()
- self._text_sub = rospy.Subscriber('talk/text', Text, self._on_text_received_cb, queue_size=1)
+ self._text_sub = self.create_subscription(Text, 'talk/text', self._on_text_received_cb, 1)
def _on_text_received_cb(self, msg):
- with self._text_sub_lock:
- if self._audio_pub.is_filtering_all_messages:
- return
+ if self._audio_pub.is_filtering_all_messages:
+ return
- try:
- if msg.text != '':
- start_time = datetime.now()
- file_path = self._voice_generator.generate(msg.text)
- frames = self._load_frames(file_path)
- processing_time_s = (datetime.now() - start_time).total_seconds()
+ try:
+ if msg.text != '':
+ start_time = datetime.now()
+ file_path = self._voice_generator.generate(msg.text)
+ frames = self._load_frames(file_path)
+ processing_time_s = (datetime.now() - start_time).total_seconds()
- self._publish_stats(msg.text, frames, processing_time_s)
+ self._publish_stats(msg.text, frames, processing_time_s)
- self._play_audio(frames)
- self._voice_generator.delete_generated_file(file_path)
- rospy.sleep(self._done_delay_s)
+ self._play_audio(frames)
+ self._voice_generator.delete_generated_file(file_path)
+ time.sleep(self._done_delay_s)
- ok = True
- except Exception as e:
- rospy.logerr(f'Unable to talk ({e})')
- ok = False
+ ok = True
+ except Exception as e:
+ self.get_logger().error(f'Unable to talk ({e})')
+ ok = False
- self._done_talking_pub.publish(Done(id=msg.id, ok=ok))
+ self._done_talking_pub.publish(Done(id=msg.id, ok=ok))
def _publish_stats(self, text, frames, processing_time_s):
stats = Statistics()
stats.text = text
stats.processing_time_s = processing_time_s
- stats.header.stamp = rospy.Time.now()
+ stats.header.stamp = self.get_clock().now().to_msg()
stats.total_samples_count = 0
for frame in frames:
@@ -106,7 +107,7 @@ def _play_audio(self, frames):
audio_frame.sampling_frequency = self._sampling_frequency
audio_frame.frame_sample_count = self._frame_sample_count
- rate = rospy.Rate(self._sampling_frequency / self._frame_sample_count)
+ rate = time_utils.Rate(self._sampling_frequency / self._frame_sample_count)
for frame in frames:
if self._audio_pub.is_filtering_all_messages:
break
@@ -124,9 +125,10 @@ def _play_audio(self, frames):
mouth_signal_msg.data = max(0.0, min(mouth_signal[0] * self._mouth_signal_gain, 1.0))
self._mouth_signal_scale_pub.publish(mouth_signal_msg)
- audio_frame.header.stamp = rospy.Time.now()
+ audio_frame.header.stamp = self.get_clock().now().to_msg()
audio_frame.data = frame.tobytes()
self._audio_pub.publish(audio_frame)
+
rate.sleep()
mouth_signal_msg.data = 0.0
@@ -134,6 +136,7 @@ def _play_audio(self, frames):
def _load_frames(self, file_path):
waveform, _ = librosa.load(file_path, sr=self._sampling_frequency, res_type='kaiser_fast')
+ waveform = librosa.to_mono(waveform)
pad = (self._frame_sample_count - (waveform.shape[0] % self._frame_sample_count)) % self._frame_sample_count
waveform = np.pad(waveform, (0, pad), 'constant', constant_values=0)
frames = np.split(waveform, np.arange(self._frame_sample_count, len(waveform), self._frame_sample_count))
@@ -156,17 +159,28 @@ def _initialize_mouth_signal_filter(self):
return mouth_signal_filter_sos, mouth_signal_filter_zi
def run(self):
- rospy.spin()
+ executer_thread_count = self._voice_generator.executer_thread_count
+ if executer_thread_count == 1:
+ rclpy.spin(self)
+ else:
+ executor = rclpy.executors.MultiThreadedExecutor(num_threads=executer_thread_count)
+ executor.add_node(self)
+ executor.spin()
def main():
- rospy.init_node('talk_node')
+ rclpy.init()
talk_node = TalkNode()
- talk_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ talk_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ talk_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/behaviors/talk/setup.py b/ros/behaviors/talk/setup.py
deleted file mode 100644
index 01b86310..00000000
--- a/ros/behaviors/talk/setup.py
+++ /dev/null
@@ -1,12 +0,0 @@
-## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from setuptools import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
- packages=['talk'],
- package_dir={'': 'src'},
-)
-
-setup(**setup_args)
diff --git a/ros/perceptions/video_analyzer/src/video_analyzer/__init__.py b/ros/behaviors/talk/talk/__init__.py
similarity index 100%
rename from ros/perceptions/video_analyzer/src/video_analyzer/__init__.py
rename to ros/behaviors/talk/talk/__init__.py
diff --git a/ros/behaviors/talk/src/talk/lib_voice_generator.py b/ros/behaviors/talk/talk/lib_voice_generator.py
similarity index 86%
rename from ros/behaviors/talk/src/talk/lib_voice_generator.py
rename to ros/behaviors/talk/talk/lib_voice_generator.py
index 27962381..1cb542bd 100644
--- a/ros/behaviors/talk/src/talk/lib_voice_generator.py
+++ b/ros/behaviors/talk/talk/lib_voice_generator.py
@@ -5,11 +5,15 @@
import json
import random
from enum import Enum
+import asyncio
from google.cloud import texttospeech
-import rospy
-from piper_ros.srv import GenerateSpeechFromText, GenerateSpeechFromTextRequest
+import rclpy
+import rclpy.node
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+
+from behavior_srvs.srv import GenerateSpeechFromText
class Language(Enum):
@@ -52,6 +56,10 @@ def __init__(self, directory: str, language: Language, gender: Gender, speaking_
@abstractmethod
def generate(self, text: str) -> str:
pass
+
+ @property
+ def executer_thread_count(self):
+ return 1
def delete_generated_file(self, file_path: str):
if os.path.exists(file_path):
@@ -102,24 +110,33 @@ def generate(self, text: str) -> str:
class PiperVoiceGenerator(VoiceGenerator):
- def __init__(self, directory: str, language: Language, gender: Gender, speaking_rate: float):
+ def __init__(self, node: rclpy.node.Node, directory: str, language: Language, gender: Gender, speaking_rate: float):
super().__init__(directory, language, gender, speaking_rate)
- self._piper_service = rospy.ServiceProxy('piper/generate_speech_from_text', GenerateSpeechFromText)
+ self._node = node
+ self._piper_service = node.create_client(GenerateSpeechFromText, 'piper/generate_speech_from_text',
+ callback_group=MutuallyExclusiveCallbackGroup())
def generate(self, text: str) -> str:
- request = GenerateSpeechFromTextRequest()
+ request = GenerateSpeechFromText.Request()
request.language = self._language.value
request.gender = self._gender.value
request.length_scale = 1.0 / self._speaking_rate
request.text = text
request.path = self._generate_random_path('.wav')
- response = self._piper_service(request)
+ future = self._piper_service.call_async(request)
+ async def wait_future():
+ return await future
+ response = asyncio.run(wait_future())
+
if not response.ok:
raise RuntimeError(response.message)
-
return request.path
+
+ @property
+ def executer_thread_count(self):
+ return 2
class CachedVoiceGenerator(VoiceGenerator):
@@ -186,5 +203,9 @@ def _get_cache_key(self, text: str) -> str:
return (f'{type(self._voice_generator).__name__}__{self._language.value}__{self._gender.value}__'
f'{self._speaking_rate}__{text}')
+ @property
+ def executer_thread_count(self):
+ return self._voice_generator.executer_thread_count
+
def delete_generated_file(self, file_path: str):
pass
diff --git a/ros/behaviors/teleoperation/CMakeLists.txt b/ros/behaviors/teleoperation/CMakeLists.txt
index 60b4bfdf..0845a600 100644
--- a/ros/behaviors/teleoperation/CMakeLists.txt
+++ b/ros/behaviors/teleoperation/CMakeLists.txt
@@ -1,205 +1,21 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
project(teleoperation)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
-## 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
- hbba_lite
- rospy
- std_msgs
- t_top
- geometry_msgs
- opentera_webrtc_ros_msgs
-)
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
-# GestureName.msg
-# Done.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# DoAction.srv
-# )
+find_package(ament_cmake REQUIRED)
-## Generate actions in the 'action' folder
-# add_action_files(
-# FILES
-# Action1.action
-# Action2.action
-# )
+find_package(rclpy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(opentera_webrtc_ros_msgs REQUIRED)
-## Generate added messages and services with any dependencies listed here
-# generate_messages()
+find_package(t_top REQUIRED)
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 gesture
- CATKIN_DEPENDS hbba_lite rospy std_msgs t_top opentera_webrtc_ros_msgs
- # DEPENDS system_lib
+install(PROGRAMS
+ scripts/teleoperation_node.py
+ DESTINATION lib/${PROJECT_NAME}
)
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
- # include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/gesture.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/gesture_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-catkin_install_python(PROGRAMS
- scripts/teleoperation_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_gesture.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_package()
diff --git a/ros/behaviors/teleoperation/README.md b/ros/behaviors/teleoperation/README.md
index 1b8c0e29..5a6c43ab 100644
--- a/ros/behaviors/teleoperation/README.md
+++ b/ros/behaviors/teleoperation/README.md
@@ -15,20 +15,20 @@ This node allows T-Top to be teleoperated using twist commands.
#### Subscribed Topics
- `daemon/motor_status` ([daemon_ros_client/MotorStatus](../../daemon_ros_client/msg/MotorStatus.msg)): The motor status.
-- `teleoperation/cmd_vel` ([geometry_msgs/Twist]): The twist commands.
+- `teleoperation/cmd_vel` ([geometry_msgs/Twist](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/Twist.html)): The twist commands.
#### Published Topics
-- `teleoperation/set_head_pose` ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)):
+- `teleoperation/set_head_pose` ([geometry_msgs/PoseStamped](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/PoseStamped.html)):
The head pose.
-- `teleoperation/set_torso_orientation` ([std_msgs/Float32](http://docs.ros.org/en/noetic/api/std_msgs/html/msg/Float32.html)): The
+- `teleoperation/set_torso_orientation` ([std_msgs/Float32](https://docs.ros.org/en/humble/p/std_msgs/interfaces/msg/Float32.html)): The
torso orientation.
#### Services
-- `pose/filter_state` ([hbba_lite/SetOnOffFilterState](../../hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `pose/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
-- `teleop_do_action` ([teleoperation/DoAction])(srv/DoAction.srv)): The name of a specific action to do:
+- `teleop_do_action` ([opentera_webrtc_ros_msgs/SetString](https://github.com/introlab/opentera-webrtc-ros/blob/ros2/opentera_webrtc_ros_msgs/srv/SetString.srv)): The name of a specific action to do:
- do_yes
- do_no
- do_maybe
diff --git a/ros/behaviors/teleoperation/package.xml b/ros/behaviors/teleoperation/package.xml
index b2e8f7e7..d36f4b81 100644
--- a/ros/behaviors/teleoperation/package.xml
+++ b/ros/behaviors/teleoperation/package.xml
@@ -1,76 +1,25 @@
-
+
+
teleoperation
0.0.0
The teleoperation package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclpy
-
-
-
- TODO
+ geometry_msgs
+ opentera_webrtc_ros_msgs
+ t_top
-
-
-
-
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- hbba_lite
- rospy
- std_msgs
- t_top
- geometry_msgs
- opentera_webrtc_ros_msgs
- hbba_lite
- rospy
- std_msgs
- t_top
- geometry_msgs
- opentera_webrtc_ros_msgs
- hbba_lite
- rospy
- std_msgs
- t_top
- opentera_webrtc_ros_msgs
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/teleoperation/scripts/teleoperation_node.py b/ros/behaviors/teleoperation/scripts/teleoperation_node.py
index 813a3995..d4387f8f 100755
--- a/ros/behaviors/teleoperation/scripts/teleoperation_node.py
+++ b/ros/behaviors/teleoperation/scripts/teleoperation_node.py
@@ -1,11 +1,21 @@
#!/usr/bin/env python3
-import rospy
-from geometry_msgs.msg import Twist
from contextlib import contextmanager
-from t_top import MovementCommands, HEAD_ZERO_Z
-from opentera_webrtc_ros_msgs.srv import SetString, SetStringRequest, SetStringResponse
+import rclpy
+import rclpy.executors
+import rclpy.node
+from geometry_msgs.msg import Twist
+from opentera_webrtc_ros_msgs.srv import SetString
+from t_top import HEAD_ZERO_Z, MovementCommands
+
+MOVE_YES_TIMEOUT = 10
+MOVE_NO_TIMEOUT = 10
+MOVE_MAYBE_TIMEOUT = 10
+MOVE_HEAD_TO_ORIGIN_TIMEOUT = 5
+MOVE_TORSO_TO_ORIGIN_TIMEOUT = 30
+MOVE_THINKING_TIMEOUT = 5
+MOVE_SAD_TIMEOUT = 5
def twist_is_null(twist: Twist) -> bool:
@@ -32,23 +42,23 @@ def __init__(self) -> None:
self.is_moving = False
-class TeleoperationNode:
+class TeleoperationNode(rclpy.node.Node):
def __init__(self):
- self._simulation = rospy.get_param('~simulation', False)
+ super().__init__('teleoperation_node')
+ self._simulation = self.declare_parameter('simulation', False).get_parameter_value().bool_value
- self._movement_commands = MovementCommands(self._simulation, namespace='teleoperation')
+ self._movement_commands = MovementCommands(self, self._simulation, namespace='teleoperation')
self._state = TeleopState()
- self._twist_sub = rospy.Subscriber(
- 'teleoperation/cmd_vel', Twist, self._on_twist_cb, queue_size=1)
+ self._twist_sub = self.create_subscription(
+ Twist, 'teleoperation/cmd_vel', self._on_twist_cb, 1)
- self._timer = rospy.Timer(rospy.Duration(
- self._movement_commands.sleep_time), self._on_timer_cb)
+ self._timer = self.create_timer(self._movement_commands.sleep_time, self._on_timer_cb)
- self._origin_serv = rospy.Service(
- "teleop_do_action", SetString, self._do_action_cb)
+ self._origin_serv = self.create_service(
+ SetString, "teleop_do_action", self._do_action_cb)
- def _on_twist_cb(self, msg):
+ def _on_twist_cb(self, msg: Twist):
if self._movement_commands.is_filtering_all_messages:
self._state.is_moving = False
return
@@ -60,7 +70,7 @@ def _on_twist_cb(self, msg):
else:
self._move(msg)
- def _on_timer_cb(self, _):
+ def _on_timer_cb(self):
if self._state.is_moving:
self._movement_commands.move_torso_speed(
speed_rad_sec_torso=self._state.current_speed.torso_angle, should_sleep=False)
@@ -90,7 +100,7 @@ def _move(self, msg: Twist) -> None:
self._state.current_speed = TeleopAngles(
torso_angle=msg.angular.z/0.15, head_angle=-1.1*msg.linear.x/0.15)
- def _do_action_cb(self, req: SetStringRequest) -> SetStringResponse:
+ def _do_action_cb(self, req: SetString.Request, res: SetString.Response) -> SetString.Response:
jump_table = {
"goto_origin": self._goto_origin,
"do_yes": self._do_yes,
@@ -102,13 +112,19 @@ def _do_action_cb(self, req: SetStringRequest) -> SetStringResponse:
if req.data in jump_table:
if self._movement_commands.is_filtering_all_messages:
- return SetStringResponse(success=False, message="HBBA filter is active")
+ res.success = False
+ res.message = "HBBA filter is active"
+ return res
jump_table[req.data]()
- return SetStringResponse(success=True, message="")
+ res.success = True
+ res.message = ""
+ return res
else:
- return SetStringResponse(success=False, message=f"'{req.data}' not in {list(jump_table.keys())}")
+ res.success = False
+ res.message = f"'{req.data}' not in {list(jump_table.keys())}"
+ return res
@contextmanager
def _pause_moving(self):
@@ -121,41 +137,46 @@ def _pause_moving(self):
def _goto_origin(self) -> None:
with self._pause_moving():
- self._movement_commands.move_head_to_origin(False)
- self._movement_commands.move_torso_to_origin(False)
+ self._movement_commands.move_head_to_origin(False, timeout=MOVE_HEAD_TO_ORIGIN_TIMEOUT)
+ self._movement_commands.move_torso_to_origin(False, timeout=MOVE_TORSO_TO_ORIGIN_TIMEOUT)
def _do_yes(self) -> None:
with self._pause_moving():
- self._movement_commands.move_yes(count=1)
+ self._movement_commands.move_yes(count=1, timeout=MOVE_YES_TIMEOUT)
def _do_no(self) -> None:
with self._pause_moving():
- self._movement_commands.move_no(count=1)
+ self._movement_commands.move_no(count=1, timeout=MOVE_NO_TIMEOUT)
def _do_maybe(self) -> None:
with self._pause_moving():
- self._movement_commands.move_maybe(count=1)
+ self._movement_commands.move_maybe(count=1, timeout=MOVE_MAYBE_TIMEOUT)
def _goto_origin_head(self) -> None:
with self._pause_moving():
- self._movement_commands.move_head_to_origin(False)
+ self._movement_commands.move_head_to_origin(False, timeout=MOVE_HEAD_TO_ORIGIN_TIMEOUT)
def _goto_origin_torso(self) -> None:
with self._pause_moving():
- self._movement_commands.move_torso_to_origin(False)
+ self._movement_commands.move_torso_to_origin(False, timeout=MOVE_TORSO_TO_ORIGIN_TIMEOUT)
def run(self):
- rospy.spin()
-
+ executor = rclpy.executors.MultiThreadedExecutor(num_threads=2)
+ executor.add_node(self)
+ executor.spin()
def main():
- rospy.init_node('teleoperation_node')
+ rclpy.init()
teleop_node = TeleoperationNode()
- teleop_node.run()
+
+ try:
+ teleop_node.run()
+ except KeyboardInterrupt:
+ pass
+ teleop_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
if __name__ == '__main__':
- try:
- main()
- except rospy.ROSInterruptException:
- pass
+ main()
diff --git a/ros/behaviors/too_close_reaction/CMakeLists.txt b/ros/behaviors/too_close_reaction/CMakeLists.txt
index b0f9313f..51ab2b59 100644
--- a/ros/behaviors/too_close_reaction/CMakeLists.txt
+++ b/ros/behaviors/too_close_reaction/CMakeLists.txt
@@ -1,208 +1,32 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(too_close_reaction)
-## Compile as C++11, supported in ROS Kinetic and newer
-# add_compile_options(-std=c++11)
-
-## 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
- cv_bridge
- geometry_msgs
- hbba_lite
- rospy
- sensor_msgs
-)
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
-# geometry_msgs# sensor_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 too_close_reaction
-# CATKIN_DEPENDS cv_bridge geometry_msgs hbba_lite rospy sensor_msgs
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/too_close_reaction.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/too_close_reaction_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-# ${catkin_LIBRARIES}
-# )
-
-#############
-## 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
-catkin_install_python(PROGRAMS
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(hbba_lite REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# Python Nodes
+install(PROGRAMS
scripts/too_close_reaction_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ DESTINATION lib/${PROJECT_NAME}
)
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_too_close_reaction.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_package()
diff --git a/ros/behaviors/too_close_reaction/README.md b/ros/behaviors/too_close_reaction/README.md
index 76a011dc..3c8b1850 100644
--- a/ros/behaviors/too_close_reaction/README.md
+++ b/ros/behaviors/too_close_reaction/README.md
@@ -10,21 +10,21 @@ This node makes T-Top move back its head when someone is too close to T-Top.
#### Parameters
-- `max_offset_m` (double): The maximum distance in meter to move back the head.
-- `too_close_start_distance_m` (double): The start distance to move back the head.
-- `too_close_end_distance_m` (double): The end distance to move back the head.
-- `pixel_ratio` (double): The pixel ratio to measure de distance.
+- `max_offset_m` (double): The maximum distance in meter to move back the head. The default value is 0.01.
+- `too_close_start_distance_m` (double): The start distance to move back the head. The default value is 0.5.
+- `too_close_end_distance_m` (double): The end distance to move back the head. The default value is 0.25.
+- `pixel_ratio` (double): The pixel ratio to measure de distance. The default value is 0.01.
#### Subscribed Topics
-- `depth_image_raw` ([sensor_msgs/Image](http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Image.html)): The depth image.
+- `depth_image_raw` ([sensor_msgs/Image](https://docs.ros.org/en/humble/p/sensor_msgs/interfaces/msg/Image.html)): The depth image.
#### Published Topics
-- `too_close_reaction/set_head_pose` ([geometry_msgs/PoseStamped](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)):
+- `too_close_reaction/set_head_pose` ([geometry_msgs/PoseStamped](https://docs.ros.org/en/humble/p/geometry_msgs/interfaces/msg/PoseStamped.html)):
The head pose offset.
#### Services
-- `depth_image_raw/filter_state` ([hbba_lite/SetOnOffFilterState](../../utils/hbba_lite/srv/SetOnOffFilterState.srv)): The HBBA filter
+- `depth_image_raw/filter_state` ([hbba_lite_srvs/SetOnOffFilterState](../../utils/hbba_lite/hbba_lite_srvs/srv/SetOnOffFilterState.srv)): The HBBA filter
state service to enable or disable the behavior.
diff --git a/ros/behaviors/too_close_reaction/package.xml b/ros/behaviors/too_close_reaction/package.xml
index ff46b2e2..04f9a2f8 100644
--- a/ros/behaviors/too_close_reaction/package.xml
+++ b/ros/behaviors/too_close_reaction/package.xml
@@ -1,74 +1,28 @@
-
+
+
too_close_reaction
0.0.0
The too_close_reaction package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclpy
+ cv_bridge
+ geometry_msgs
+ hbba_lite
+ sensor_msgs
-
-
-
- TODO
+ rosidl_default_runtime
+ ros2launch
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- cv_bridge
- geometry_msgs
- hbba_lite
- rospy
- sensor_msgs
- cv_bridge
- geometry_msgs
- hbba_lite
- rospy
- sensor_msgs
- cv_bridge
- geometry_msgs
- hbba_lite
- rospy
- sensor_msgs
-
-
-
-
-
+ ament_cmake
diff --git a/ros/behaviors/too_close_reaction/scripts/too_close_reaction_node.py b/ros/behaviors/too_close_reaction/scripts/too_close_reaction_node.py
index f9f208ff..7586fe10 100755
--- a/ros/behaviors/too_close_reaction/scripts/too_close_reaction_node.py
+++ b/ros/behaviors/too_close_reaction/scripts/too_close_reaction_node.py
@@ -2,7 +2,9 @@
import numpy as np
import cv2
-import rospy
+import rclpy
+import rclpy.node
+
from cv_bridge import CvBridge
from geometry_msgs.msg import PoseStamped
@@ -11,24 +13,26 @@
import hbba_lite
-class TooCloseReactionNode:
+class TooCloseReactionNode(rclpy.node.Node):
def __init__(self):
- self._max_offset_m = rospy.get_param('~max_offset_m', 0.01)
- self._too_close_start_distance_m = rospy.get_param('~too_close_start_distance_m', 0.5)
- self._too_close_end_distance_m = rospy.get_param('~too_close_end_distance_m', 0.25)
- self._pixel_ratio = rospy.get_param('~pixel_ratio', 0.01)
+ super().__init__('too_close_reaction_node')
+
+ self._max_offset_m = self.declare_parameter('max_offset_m', 0.01).get_parameter_value().double_value
+ self._too_close_start_distance_m = self.declare_parameter('too_close_start_distance_m', 0.5).get_parameter_value().double_value
+ self._too_close_end_distance_m = self.declare_parameter('too_close_end_distance_m', 0.25).get_parameter_value().double_value
+ self._pixel_ratio = self.declare_parameter('pixel_ratio', 0.01).get_parameter_value().double_value
self._cv_bridge = CvBridge()
self._current_offset_m = 0.0
- self._head_pose_pub = rospy.Publisher('too_close_reaction/set_head_pose', PoseStamped, queue_size=1)
- self._depth_image_sub = hbba_lite.OnOffHbbaSubscriber('depth_image_raw', Image, self._image_cb, queue_size=1)
+ self._head_pose_pub = self.create_publisher(PoseStamped, 'too_close_reaction/set_head_pose', 1)
+ self._depth_image_sub = hbba_lite.OnOffHbbaSubscriber(self, Image, 'depth_image_raw', self._image_cb, 1)
self._depth_image_sub.on_filter_state_changed(self._hbba_filter_state_cb)
def _image_cb(self, msg):
if msg.encoding != '16UC1':
- rospy.logerr('Invalid depth image encoding')
+ self.get_logger().error('Invalid depth image encoding')
depth_image = self._cv_bridge.imgmsg_to_cv2(msg, '16UC1')
distance_m = self._compute_distance(depth_image) - self._current_offset_m
@@ -63,29 +67,34 @@ def _send_pose(self, offset_m):
pose_msg = PoseStamped()
pose_msg.header.frame_id = 'stewart_base'
- pose_msg.pose.position.x = -offset_m
- pose_msg.pose.position.y = 0
- pose_msg.pose.position.z = 0
+ pose_msg.pose.position.x = float(-offset_m)
+ pose_msg.pose.position.y = 0.0
+ pose_msg.pose.position.z = 0.0
- pose_msg.pose.orientation.x = 0
- pose_msg.pose.orientation.y = 0
- pose_msg.pose.orientation.z = 0
- pose_msg.pose.orientation.w = 1
+ pose_msg.pose.orientation.x = 0.0
+ pose_msg.pose.orientation.y = 0.0
+ pose_msg.pose.orientation.z = 0.0
+ pose_msg.pose.orientation.w = 1.0
self._head_pose_pub.publish(pose_msg)
def run(self):
- rospy.spin()
+ rclpy.spin(self)
def main():
- rospy.init_node('too_close_reaction_node')
+ rclpy.init()
too_close_reaction_node = TooCloseReactionNode()
- too_close_reaction_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ too_close_reaction_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ too_close_reaction_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/daemon_ros_client/CMakeLists.txt b/ros/daemon_ros_client/CMakeLists.txt
index 3c997763..404f2dff 100644
--- a/ros/daemon_ros_client/CMakeLists.txt
+++ b/ros/daemon_ros_client/CMakeLists.txt
@@ -1,148 +1,44 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(daemon_ros_client)
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-## 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
- geometry_msgs
- message_generation
- roscpp
- sensor_msgs
- std_msgs
- tf2
- tf2_ros
-)
-
-## 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()
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
-################################################
-## Declare ROS messages, services and actions ##
-################################################
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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 ...)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
-## Generate messages in the 'msg' folder
-add_message_files(
- FILES
- BaseStatus.msg
- MotorStatus.msg
- LedColor.msg
- LedColors.msg
-)
-
-## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
-## 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
- geometry_msgs
- sensor_msgs
- std_msgs
+# Generate messages
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/BaseStatus.msg"
+ "msg/MotorStatus.msg"
+ "msg/LedColor.msg"
+ "msg/LedColors.msg"
+ DEPENDENCIES std_msgs geometry_msgs sensor_msgs
)
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 daemon_ros_client
-# CATKIN_DEPENDS geometry_msgs message_generation roscpp sensor_msgs std_msgs
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
include(../../system/common/common.cmake)
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/daemon_ros_client.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
+# C++ Nodes
set(app_headers
src/DaemonRosClientNode.h
@@ -156,75 +52,10 @@ set (app_sources
qt5_wrap_cpp(app_headers_moc_srcs ${app_headers})
-add_executable(${PROJECT_NAME}_node ${app_headers} ${app_sources} ${app_headers_moc_srcs})
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}_node
- ${catkin_LIBRARIES}
- serial_communication_common
-)
-
-#############
-## 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
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_daemon_ros_client.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+add_executable(daemon_ros_client_node ${app_headers} ${app_sources} ${app_headers_moc_srcs})
+ament_target_dependencies(daemon_ros_client_node rclcpp std_msgs geometry_msgs sensor_msgs tf2 tf2_ros)
+target_link_libraries(daemon_ros_client_node serial_communication_common)
+rosidl_target_interfaces(daemon_ros_client_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
+install(TARGETS daemon_ros_client_node DESTINATION lib/${PROJECT_NAME})
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ament_package()
diff --git a/ros/daemon_ros_client/launch/client.launch b/ros/daemon_ros_client/launch/client.launch
deleted file mode 100644
index 85a90f28..00000000
--- a/ros/daemon_ros_client/launch/client.launch
+++ /dev/null
@@ -1,5 +0,0 @@
-
-
-
-
-
diff --git a/ros/daemon_ros_client/msg/BaseStatus.msg b/ros/daemon_ros_client/msg/BaseStatus.msg
index e94d5e1a..c33c698f 100644
--- a/ros/daemon_ros_client/msg/BaseStatus.msg
+++ b/ros/daemon_ros_client/msg/BaseStatus.msg
@@ -1,4 +1,4 @@
-Header header
+std_msgs/Header header
bool is_psu_connected
bool has_charger_error
bool is_battery_charging
diff --git a/ros/daemon_ros_client/msg/MotorStatus.msg b/ros/daemon_ros_client/msg/MotorStatus.msg
index b99b436c..2f9df2ad 100644
--- a/ros/daemon_ros_client/msg/MotorStatus.msg
+++ b/ros/daemon_ros_client/msg/MotorStatus.msg
@@ -1,4 +1,4 @@
-Header header
+std_msgs/Header header
float32 torso_orientation
int16 torso_servo_speed
diff --git a/ros/daemon_ros_client/package.xml b/ros/daemon_ros_client/package.xml
index 2637506a..bae8edb9 100644
--- a/ros/daemon_ros_client/package.xml
+++ b/ros/daemon_ros_client/package.xml
@@ -1,78 +1,31 @@
-
+
+
daemon_ros_client
0.0.0
The daemon_ros_client package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rosidl_default_generators
+ rclcpp
-
-
-
- TODO
+ std_msgs
+ geometry_msgs
+ sensor_msgs
+ tf2
+ tf2_ros
+ rosidl_default_runtime
+ ros2launch
+ rosidl_interface_packages
-
-
-
-
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- geometry_msgs
- message_generation
- roscpp
- sensor_msgs
- std_msgs
- tf2
- tf2_ros
- geometry_msgs
- roscpp
- sensor_msgs
- std_msgs
- tf2
- tf2_ros
- geometry_msgs
- roscpp
- sensor_msgs
- std_msgs
- tf2
- tf2_ros
-
-
-
-
-
+ ament_cmake
diff --git a/ros/daemon_ros_client/src/DaemonRosClientNode.cpp b/ros/daemon_ros_client/src/DaemonRosClientNode.cpp
index c6b0da68..5c19c536 100644
--- a/ros/daemon_ros_client/src/DaemonRosClientNode.cpp
+++ b/ros/daemon_ros_client/src/DaemonRosClientNode.cpp
@@ -2,25 +2,38 @@
#include "QtUtils.h"
-DaemonRosClientNode::DaemonRosClientNode(int &argc, char* argv[], ros::NodeHandle& nodeHandle, DaemonRosClientNodeConfiguration configuration)
- : QCoreApplication(argc, argv), m_websocketProtocolWrapper(nullptr), m_nodeHandle(nodeHandle), m_configuration(move(configuration))
+DaemonRosClientNode::DaemonRosClientNode() : rclcpp::Node("daemon_ros_client_node"), m_tfBroadcaster(*this)
{
- initWebSocketProtocolWrapper();
+ m_baseLinkTorsoBaseDeltaZ = declare_parameter("base_link_torso_base_delta_z", 0.0);
+
initROS();
+ initWebSocketProtocolWrapper();
}
void DaemonRosClientNode::initROS()
{
- m_baseStatusPub = m_nodeHandle.advertise("daemon/base_status", PubQueueSize);
- m_startButtonPressedPub = m_nodeHandle.advertise("daemon/start_button_pressed", PubQueueSize);
- m_stopButtonPressedPub = m_nodeHandle.advertise("daemon/stop_button_pressed", PubQueueSize);
- m_imuPub = m_nodeHandle.advertise("daemon/imu/data_raw", PubQueueSize);
- m_motorStatusPub = m_nodeHandle.advertise("daemon/motor_status", PubQueueSize);
-
- m_setVolumeSub = m_nodeHandle.subscribe("daemon/set_volume", SubQueueSize, &DaemonRosClientNode::setVolumeCallback, this);
- m_setLedColorsSub = m_nodeHandle.subscribe("daemon/set_led_colors", SubQueueSize, &DaemonRosClientNode::setLedColorsCallback, this);
- m_setTorsoOrientationSub = m_nodeHandle.subscribe("daemon/set_torso_orientation", SubQueueSize, &DaemonRosClientNode::setTorsoOrientationCallback, this);
- m_setHeadPoseSub = m_nodeHandle.subscribe("daemon/set_head_pose", SubQueueSize, &DaemonRosClientNode::setHeadPoseCallback, this);
+ m_baseStatusPub = create_publisher("daemon/base_status", PubQueueSize);
+ m_startButtonPressedPub = create_publisher("daemon/start_button_pressed", PubQueueSize);
+ m_stopButtonPressedPub = create_publisher("daemon/stop_button_pressed", PubQueueSize);
+ m_imuPub = create_publisher("daemon/imu/data_raw", PubQueueSize);
+ m_motorStatusPub = create_publisher("daemon/motor_status", PubQueueSize);
+
+ m_setVolumeSub = create_subscription(
+ "daemon/set_volume",
+ SubQueueSize,
+ [this](const std_msgs::msg::UInt8::SharedPtr msg) { setVolumeCallback(msg); });
+ m_setLedColorsSub = create_subscription(
+ "daemon/set_led_colors",
+ SubQueueSize,
+ [this](const daemon_ros_client::msg::LedColors::SharedPtr msg) { setLedColorsCallback(msg); });
+ m_setTorsoOrientationSub = create_subscription(
+ "daemon/set_torso_orientation",
+ SubQueueSize,
+ [this](const std_msgs::msg::Float32::SharedPtr msg) { setTorsoOrientationCallback(msg); });
+ m_setHeadPoseSub = create_subscription(
+ "daemon/set_head_pose",
+ SubQueueSize,
+ [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { setHeadPoseCallback(msg); });
}
void DaemonRosClientNode::initWebSocketProtocolWrapper()
@@ -30,13 +43,33 @@ void DaemonRosClientNode::initWebSocketProtocolWrapper()
// Connect only useful signals
// WARNING Using direct connection, handleXXX functions must be thread safe)
- connect(m_websocketProtocolWrapper, &WebSocketProtocolWrapper::newBaseStatus, this, &DaemonRosClientNode::handleBaseStatus, Qt::DirectConnection);
- connect(m_websocketProtocolWrapper, &WebSocketProtocolWrapper::newButtonPressed, this, &DaemonRosClientNode::handleButtonPressed, Qt::DirectConnection);
- connect(m_websocketProtocolWrapper, &WebSocketProtocolWrapper::newImuData, this, &DaemonRosClientNode::handleImuData, Qt::DirectConnection);
- connect(m_websocketProtocolWrapper, &WebSocketProtocolWrapper::newMotorStatus, this, &DaemonRosClientNode::handleMotorStatus, Qt::DirectConnection);
+ connect(
+ m_websocketProtocolWrapper,
+ &WebSocketProtocolWrapper::newBaseStatus,
+ this,
+ &DaemonRosClientNode::handleBaseStatus,
+ Qt::DirectConnection);
+ connect(
+ m_websocketProtocolWrapper,
+ &WebSocketProtocolWrapper::newButtonPressed,
+ this,
+ &DaemonRosClientNode::handleButtonPressed,
+ Qt::DirectConnection);
+ connect(
+ m_websocketProtocolWrapper,
+ &WebSocketProtocolWrapper::newImuData,
+ this,
+ &DaemonRosClientNode::handleImuData,
+ Qt::DirectConnection);
+ connect(
+ m_websocketProtocolWrapper,
+ &WebSocketProtocolWrapper::newMotorStatus,
+ this,
+ &DaemonRosClientNode::handleMotorStatus,
+ Qt::DirectConnection);
}
-void DaemonRosClientNode::setVolumeCallback(const std_msgs::UInt8::ConstPtr& msg)
+void DaemonRosClientNode::setVolumeCallback(const std_msgs::msg::UInt8::SharedPtr& msg)
{
SetVolumePayload payload;
payload.volume = msg->data;
@@ -51,10 +84,10 @@ void DaemonRosClientNode::setVolumeCallback(const std_msgs::UInt8::ConstPtr& msg
});
}
-void DaemonRosClientNode::setLedColorsCallback(const daemon_ros_client::LedColors::ConstPtr& msg)
+void DaemonRosClientNode::setLedColorsCallback(const daemon_ros_client::msg::LedColors::SharedPtr& msg)
{
SetLedColorsPayload payload;
- for (size_t i = 0; i < SetLedColorsPayload::LED_COUNT; i++)
+ for (size_t i = 0; i < SetLedColorsPayload::LED_COUNT; i++)
{
payload.colors[i].red = msg->colors[i].red;
payload.colors[i].green = msg->colors[i].green;
@@ -71,7 +104,7 @@ void DaemonRosClientNode::setLedColorsCallback(const daemon_ros_client::LedColor
});
}
-void DaemonRosClientNode::setTorsoOrientationCallback(const std_msgs::Float32::ConstPtr& msg)
+void DaemonRosClientNode::setTorsoOrientationCallback(const std_msgs::msg::Float32::SharedPtr& msg)
{
SetTorsoOrientationPayload payload;
payload.torsoOrientation = msg->data;
@@ -86,11 +119,11 @@ void DaemonRosClientNode::setTorsoOrientationCallback(const std_msgs::Float32::C
});
}
-void DaemonRosClientNode::setHeadPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+void DaemonRosClientNode::setHeadPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr& msg)
{
if (msg->header.frame_id != HEAD_POSE_FRAME_ID)
{
- ROS_ERROR_STREAM("Invalid head pose frame id (" << msg->header.frame_id << ")");
+ RCLCPP_ERROR_STREAM(get_logger(), "Invalid head pose frame id (" << msg->header.frame_id << ")");
return;
}
@@ -116,8 +149,8 @@ void DaemonRosClientNode::setHeadPoseCallback(const geometry_msgs::PoseStamped::
void DaemonRosClientNode::handleBaseStatus(Device source, const BaseStatusPayload& payload)
{
// WARNING must be tread safe, called from Qt Thread
- daemon_ros_client::BaseStatus msg;
- msg.header.stamp = ros::Time::now();
+ daemon_ros_client::msg::BaseStatus msg;
+ msg.header.stamp = get_clock()->now();
msg.is_psu_connected = payload.isPsuConnected;
msg.has_charger_error = payload.hasChargerError;
msg.is_battery_charging = payload.isBatteryCharging;
@@ -134,7 +167,7 @@ void DaemonRosClientNode::handleBaseStatus(Device source, const BaseStatusPayloa
msg.volume = payload.volume;
msg.maximum_volume = payload.maximumVolume;
- m_baseStatusPub.publish(msg);
+ m_baseStatusPub->publish(msg);
}
void DaemonRosClientNode::handleButtonPressed(Device source, const ButtonPressedPayload& payload)
@@ -143,13 +176,13 @@ void DaemonRosClientNode::handleButtonPressed(Device source, const ButtonPressed
switch (payload.button)
{
case Button::START:
- m_startButtonPressedPub.publish(std_msgs::Empty());
+ m_startButtonPressedPub->publish(std_msgs::msg::Empty());
break;
case Button::STOP:
- m_stopButtonPressedPub.publish(std_msgs::Empty());
+ m_stopButtonPressedPub->publish(std_msgs::msg::Empty());
break;
default:
- ROS_ERROR_STREAM("Not handled buttons (" << static_cast(payload.button) << ")");
+ RCLCPP_ERROR_STREAM(get_logger(), "Not handled buttons (" << static_cast(payload.button) << ")");
break;
}
}
@@ -157,31 +190,31 @@ void DaemonRosClientNode::handleButtonPressed(Device source, const ButtonPressed
void DaemonRosClientNode::handleImuData(Device source, const ImuDataPayload& payload)
{
// WARNING must be tread safe, called from Qt Thread
- sensor_msgs::Imu msg;
- msg.header.stamp = ros::Time::now();
+ sensor_msgs::msg::Imu msg;
+ msg.header.stamp = get_clock()->now();
msg.header.frame_id = "dynamixel_control_imu";
msg.orientation.x = 0;
msg.orientation.y = 0;
msg.orientation.z = 0;
msg.orientation.w = 0;
- msg.orientation_covariance.assign(-1.0);
+ msg.orientation_covariance.fill(-1.0);
msg.angular_velocity.x = payload.angularRateX;
msg.angular_velocity.y = payload.angularRateY;
msg.angular_velocity.z = payload.angularRateZ;
- msg.angular_velocity_covariance.assign(0.0);
+ msg.angular_velocity_covariance.fill(0.0);
msg.linear_acceleration.x = payload.accelerationX;
msg.linear_acceleration.y = payload.accelerationY;
msg.linear_acceleration.z = payload.accelerationZ;
- msg.linear_acceleration_covariance.assign(0.0);
+ msg.linear_acceleration_covariance.fill(0.0);
- m_imuPub.publish(msg);
+ m_imuPub->publish(msg);
}
void DaemonRosClientNode::handleMotorStatus(Device source, const MotorStatusPayload& payload)
{
// WARNING must be tread safe, called from Qt Thread
- daemon_ros_client::MotorStatus msg;
- msg.header.stamp = ros::Time::now();
+ daemon_ros_client::msg::MotorStatus msg;
+ msg.header.stamp = get_clock()->now();
msg.torso_orientation = payload.torsoOrientation;
msg.torso_servo_speed = payload.torsoServoSpeed;
@@ -210,19 +243,19 @@ void DaemonRosClientNode::handleMotorStatus(Device source, const MotorStatusPayl
sendTorsoTf(msg.header.stamp, msg.torso_orientation);
sendHeadTf(msg.header.stamp, msg.head_pose);
- m_motorStatusPub.publish(msg);
+ m_motorStatusPub->publish(msg);
}
-void DaemonRosClientNode::sendTorsoTf(const ros::Time& stamp, float torsoOrientation)
+void DaemonRosClientNode::sendTorsoTf(const rclcpp::Time& stamp, float torsoOrientation)
{
- geometry_msgs::TransformStamped transformStamped;
+ geometry_msgs::msg::TransformStamped transformStamped;
transformStamped.header.stamp = stamp;
transformStamped.header.frame_id = "base_link";
transformStamped.child_frame_id = "torso_base";
transformStamped.transform.translation.x = 0;
transformStamped.transform.translation.y = 0;
- transformStamped.transform.translation.z = m_configuration.baseLinkTorsoBaseDeltaZ;
+ transformStamped.transform.translation.z = m_baseLinkTorsoBaseDeltaZ;
tf2::Quaternion q;
q.setRPY(0, 0, torsoOrientation);
@@ -234,9 +267,9 @@ void DaemonRosClientNode::sendTorsoTf(const ros::Time& stamp, float torsoOrienta
m_tfBroadcaster.sendTransform(transformStamped);
}
-void DaemonRosClientNode::sendHeadTf(const ros::Time& stamp, const geometry_msgs::Pose& pose)
+void DaemonRosClientNode::sendHeadTf(const rclcpp::Time& stamp, const geometry_msgs::msg::Pose& pose)
{
- geometry_msgs::TransformStamped transformStamped;
+ geometry_msgs::msg::TransformStamped transformStamped;
transformStamped.header.stamp = stamp;
transformStamped.header.frame_id = HEAD_POSE_FRAME_ID;
@@ -256,7 +289,7 @@ void DaemonRosClientNode::sendHeadTf(const ros::Time& stamp, const geometry_msgs
void DaemonRosClientNode::cleanup()
{
SetLedColorsPayload payload;
- for (size_t i = 0; i < SetLedColorsPayload::LED_COUNT; i++)
+ for (size_t i = 0; i < SetLedColorsPayload::LED_COUNT; i++)
{
payload.colors[i].red = 0;
payload.colors[i].green = 0;
diff --git a/ros/daemon_ros_client/src/DaemonRosClientNode.h b/ros/daemon_ros_client/src/DaemonRosClientNode.h
index 4a514fc8..cdee2545 100644
--- a/ros/daemon_ros_client/src/DaemonRosClientNode.h
+++ b/ros/daemon_ros_client/src/DaemonRosClientNode.h
@@ -1,24 +1,23 @@
#ifndef _DAEMON_ROS_CLIENT_NODE_H_
#define _DAEMON_ROS_CLIENT_NODE_H_
-#include
#include "WebSocketProtocolWrapper.h"
-#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
@@ -29,59 +28,45 @@ constexpr uint32_t PubQueueSize = 1;
constexpr uint32_t SubQueueSize = 10;
constexpr const char* HEAD_POSE_FRAME_ID = "stewart_base";
-struct DaemonRosClientNodeConfiguration
-{
- double baseLinkTorsoBaseDeltaZ;
-
- DaemonRosClientNodeConfiguration() : baseLinkTorsoBaseDeltaZ(0.0)
- {
-
- }
-};
-
-class DaemonRosClientNode : public QCoreApplication
+class DaemonRosClientNode : public QObject, public rclcpp::Node
{
+ Q_OBJECT
- WebSocketProtocolWrapper *m_websocketProtocolWrapper;
+ double m_baseLinkTorsoBaseDeltaZ;
- ros::NodeHandle& m_nodeHandle;
- DaemonRosClientNodeConfiguration m_configuration;
+ WebSocketProtocolWrapper* m_websocketProtocolWrapper;
- ros::Publisher m_baseStatusPub;
- ros::Publisher m_startButtonPressedPub;
- ros::Publisher m_stopButtonPressedPub;
- ros::Publisher m_imuPub;
- ros::Publisher m_motorStatusPub;
+ rclcpp::Publisher::SharedPtr m_baseStatusPub;
+ rclcpp::Publisher::SharedPtr m_startButtonPressedPub;
+ rclcpp::Publisher::SharedPtr m_stopButtonPressedPub;
+ rclcpp::Publisher::SharedPtr m_imuPub;
+ rclcpp::Publisher::SharedPtr m_motorStatusPub;
- ros::Subscriber m_setVolumeSub;
- ros::Subscriber m_setLedColorsSub;
- ros::Subscriber m_setTorsoOrientationSub;
- ros::Subscriber m_setHeadPoseSub;
+ rclcpp::Subscription::SharedPtr m_setVolumeSub;
+ rclcpp::Subscription::SharedPtr m_setLedColorsSub;
+ rclcpp::Subscription::SharedPtr m_setTorsoOrientationSub;
+ rclcpp::Subscription::SharedPtr m_setHeadPoseSub;
tf2_ros::TransformBroadcaster m_tfBroadcaster;
- Q_OBJECT
-
- public:
-
- DaemonRosClientNode(int &argc, char* argv[], ros::NodeHandle& nodeHandle, DaemonRosClientNodeConfiguration configuration);
+public:
+ DaemonRosClientNode();
void cleanup();
- private:
-
+private:
void initROS();
void initWebSocketProtocolWrapper();
- void setVolumeCallback(const std_msgs::UInt8::ConstPtr& msg);
- void setLedColorsCallback(const daemon_ros_client::LedColors::ConstPtr& msg);
- void setTorsoOrientationCallback(const std_msgs::Float32::ConstPtr& msg);
- void setHeadPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
+ void setVolumeCallback(const std_msgs::msg::UInt8::SharedPtr& msg);
+ void setLedColorsCallback(const daemon_ros_client::msg::LedColors::SharedPtr& msg);
+ void setTorsoOrientationCallback(const std_msgs::msg::Float32::SharedPtr& msg);
+ void setHeadPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr& msg);
void handleBaseStatus(Device source, const BaseStatusPayload& payload);
void handleButtonPressed(Device source, const ButtonPressedPayload& payload);
void handleImuData(Device source, const ImuDataPayload& payload);
void handleMotorStatus(Device source, const MotorStatusPayload& payload);
- void sendTorsoTf(const ros::Time& stamp, float torsoOrientation);
- void sendHeadTf(const ros::Time& stamp, const geometry_msgs::Pose& pose);
+ void sendTorsoTf(const rclcpp::Time& stamp, float torsoOrientation);
+ void sendHeadTf(const rclcpp::Time& stamp, const geometry_msgs::msg::Pose& pose);
};
-#endif //_DAEMON_ROS_CLIENT_NODE_H_
+#endif //_DAEMON_ROS_CLIENT_NODE_H_
diff --git a/ros/daemon_ros_client/src/daemon_ros_client_node.cpp b/ros/daemon_ros_client/src/daemon_ros_client_node.cpp
index 462e888c..ec69e922 100644
--- a/ros/daemon_ros_client/src/daemon_ros_client_node.cpp
+++ b/ros/daemon_ros_client/src/daemon_ros_client_node.cpp
@@ -1,5 +1,8 @@
#include "DaemonRosClientNode.h"
+
+#include
+
#include
#include
@@ -28,31 +31,27 @@ void catchUnixSignals(std::initializer_list quitSignals)
int main(int argc, char** argv)
{
- ros::init(argc, argv, "daemon_ros_client_node");
+ QCoreApplication app(argc, argv);
- ros::NodeHandle nodeHandle;
- ros::NodeHandle privateNodeHandle("~");
+ rclcpp::init(argc, argv);
- DaemonRosClientNodeConfiguration configuration;
+ auto node = std::make_shared();
- if (!privateNodeHandle.getParam("base_link_torso_base_delta_z", configuration.baseLinkTorsoBaseDeltaZ))
- {
- ROS_ERROR("The parameter base_link_torso_base_delta_z is required.");
- return -1;
- }
+ catchUnixSignals({SIGQUIT, SIGINT, SIGTERM, SIGHUP});
// Run ROS in background
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- catchUnixSignals({SIGQUIT, SIGINT, SIGTERM, SIGHUP});
+ rclcpp::executors::SingleThreadedExecutor rosExecutor;
+ rosExecutor.add_node(node);
+ std::thread spinThread([&rosExecutor]() { rosExecutor.spin(); });
// Initialize and start Qt App
- DaemonRosClientNode app(argc, argv, nodeHandle, configuration);
- app.exec();
+ int returnCode = app.exec();
+
+ node->cleanup();
+ rosExecutor.cancel();
+ spinThread.join();
- app.cleanup();
+ rclcpp::shutdown();
- // Stop ROS spinner
- spinner.stop();
+ return returnCode;
}
diff --git a/ros/demos/connect4/CMakeLists.txt b/ros/demos/connect4/CMakeLists.txt
index 6e43d399..30e54162 100644
--- a/ros/demos/connect4/CMakeLists.txt
+++ b/ros/demos/connect4/CMakeLists.txt
@@ -1,158 +1,40 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
project(connect4)
-## Set compile options
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-## 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
- daemon_ros_client
- hbba_lite
- roscpp
- sensor_msgs
- t_top_hbba_lite
- opentera_webrtc_ros_msgs
- led_animations
- daemon_ros_client
- opentera_webrtc_ros
- opentera_webrtc_ros_msgs
- tf
- t_top
- std_msgs
- geometry_msgs
- video_analyzer
-)
-
-## System dependencies are found with CMake's conventions
-
-add_compile_definitions(WEBRTC_POSIX QT_NO_KEYWORDS)
-
-set(connect4_qt_components Core Widgets Network WebSockets)
-find_package(Qt5 COMPONENTS ${connect4_qt_components} REQUIRED)
-
-
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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
-# sensor_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 connect4
-# CATKIN_DEPENDS daemon_ros_client hbba_lite roscpp sensor_msgs t_top_hbba_lite
-# DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/connect4.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(daemon_ros_client REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(t_top_hbba_lite REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(perception_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(opentera_webrtc_ros_msgs REQUIRED)
+
+set(connect4_components Core Widgets Gui WebSockets Network)
+find_package(Qt5 COMPONENTS ${connect4_components} REQUIRED)
+
+# C++ Nodes
set(moc_headers
- src/ImageDisplay.h
src/Connect4Widget.h
+ src/ImageDisplay.h
)
set(other_headers
@@ -168,76 +50,35 @@ set(srcs
qt5_wrap_cpp(project_moc_srcs ${moc_headers})
add_executable(${PROJECT_NAME}_node ${srcs} ${uis} ${moc_headers} ${other_headers} ${project_moc_uis} ${project_moc_srcs})
-qt5_use_modules(${PROJECT_NAME}_node ${connect4_qt_components})
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}_node
- ${catkin_LIBRARIES}
- ${connect4_qt_components_link}
+qt5_use_modules(${PROJECT_NAME}_node ${connect4_components})
+ament_target_dependencies(${PROJECT_NAME}_node
+ rclcpp
+ t_top_hbba_lite
+ perception_msgs
+ opentera_webrtc_ros_msgs
)
+install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME})
-#############
-## 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
-catkin_install_python(PROGRAMS
- scripts/nearest_face_orientation_node.py
+# Python Nodes
+install(PROGRAMS
scripts/nearest_face_following_orientation_replication_node.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ scripts/nearest_face_orientation_node.py
+ DESTINATION lib/${PROJECT_NAME}
)
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_connect4.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)
+# Launch files
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ # the following line skips the linter which checks for copyrights
+ # comment the line when a copyright and license is added to all source files
+ set(ament_cmake_copyright_FOUND TRUE)
+ # the following line skips cpplint (only works in a git repo)
+ # comment the line when this package is in a git repo and when
+ # a copyright and license is added to all source files
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/ros/demos/connect4/launch/connect4.launch b/ros/demos/connect4/launch/connect4.launch.xml
similarity index 65%
rename from ros/demos/connect4/launch/connect4.launch
rename to ros/demos/connect4/launch/connect4.launch.xml
index 6696cfc3..9729fe2e 100644
--- a/ros/demos/connect4/launch/connect4.launch
+++ b/ros/demos/connect4/launch/connect4.launch.xml
@@ -9,26 +9,26 @@
-
-
-
+
+
+
-
+
-
-
-
+
+
+
-
-
-
+
+
+
@@ -42,17 +42,17 @@
-
+
-
-
-
-
+
+
+
+
-
+
@@ -63,9 +63,9 @@
-
+
-
+
@@ -81,38 +81,38 @@
-
+
-
-
-
+
+
+
-
+
-
+
-
+
-
-
+
+
-
-
+
+
diff --git a/ros/demos/connect4/launch/test_face_orientation.launch b/ros/demos/connect4/launch/test_face_orientation.launch.xml
similarity index 75%
rename from ros/demos/connect4/launch/test_face_orientation.launch
rename to ros/demos/connect4/launch/test_face_orientation.launch.xml
index 4bdcc63e..020b03bb 100644
--- a/ros/demos/connect4/launch/test_face_orientation.launch
+++ b/ros/demos/connect4/launch/test_face_orientation.launch.xml
@@ -1,29 +1,34 @@
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
-
+
@@ -32,9 +37,9 @@
-
+
-
+
@@ -45,20 +50,17 @@
-
-
+
-
-
+
-
diff --git a/ros/demos/connect4/package.xml b/ros/demos/connect4/package.xml
index fda1b92a..4b99331e 100644
--- a/ros/demos/connect4/package.xml
+++ b/ros/demos/connect4/package.xml
@@ -1,104 +1,29 @@
-
+
+
connect4
0.0.0
- The connect4 package
+ Connect4 game
+ Marc-Antoine Maheux
+ GPL-3.0 license
+
+ ament_cmake
+
+ daemon_ros_client
+ sensor_msgs
+ rclcpp
+ rclpy
+ t_top_hbba_lite
+ tf2
+ tf2_ros
+ perception_msgs
+ geometry_msgs
+ opentera_webrtc_ros_msgs
+
+ ament_lint_auto
+ ament_lint_common
-
-
-
- marc-antoine
-
-
-
-
-
- TODO
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- daemon_ros_client
- hbba_lite
- roscpp
- sensor_msgs
- t_top_hbba_lite
- opentera_webrtc_ros_msgs
- led_animations
- daemon_ros_client
- opentera_webrtc_ros
- opentera_webrtc_ros_msgs
- tf
- t_top
- std_msgs
- geometry_msgs
- video_analyzer
- daemon_ros_client
- hbba_lite
- roscpp
- sensor_msgs
- t_top_hbba_lite
- opentera_webrtc_ros_msgs
- led_animations
- daemon_ros_client
- opentera_webrtc_ros
- opentera_webrtc_ros_msgs
- tf
- t_top
- std_msgs
- geometry_msgs
- video_analyzer
- daemon_ros_client
- hbba_lite
- roscpp
- sensor_msgs
- t_top_hbba_lite
- opentera_webrtc_ros_msgs
- led_animations
- daemon_ros_client
- opentera_webrtc_ros
- opentera_webrtc_ros_msgs
- tf
- t_top
- std_msgs
- geometry_msgs
- video_analyzer
-
-
-
-
-
+ ament_cmake
diff --git a/ros/demos/connect4/scripts/nearest_face_following_orientation_replication_node.py b/ros/demos/connect4/scripts/nearest_face_following_orientation_replication_node.py
index 8ca381c6..919079e5 100755
--- a/ros/demos/connect4/scripts/nearest_face_following_orientation_replication_node.py
+++ b/ros/demos/connect4/scripts/nearest_face_following_orientation_replication_node.py
@@ -5,10 +5,18 @@
import numpy as np
-import rospy
-import tf
+
+import rclpy
+import rclpy.node
+import rclpy.executors
+
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+import tf2_geometry_msgs
+
from geometry_msgs.msg import PointStamped
-from video_analyzer.msg import VideoAnalysis
+from perception_msgs.msg import VideoAnalysis
from std_msgs.msg import String
from opentera_webrtc_ros_msgs.msg import PeerData
@@ -18,47 +26,54 @@
PERSON_POSE_NOSE_INDEX = 0
-class NearestFaceFollowingOrientationReplicationNode:
+class NearestFaceFollowingOrientationReplicationNode(rclpy.node.Node):
def __init__(self):
- self._simulation = rospy.get_param('~simulation')
- self._rate = rospy.Rate(rospy.get_param('~control_frequency'))
- self._torso_control_alpha = rospy.get_param('~torso_control_alpha')
- self._head_control_alpha = rospy.get_param('~head_control_alpha')
- self._head_control_pitch_up_alpha_gain = rospy.get_param('~head_control_pitch_up_alpha_gain')
- self._min_head_roll = rospy.get_param('~min_head_roll_rad')
- self._max_head_roll = rospy.get_param('~max_head_roll_rad')
- self._min_head_pitch = rospy.get_param('~min_head_pitch_rad')
- self._max_head_pitch = rospy.get_param('~max_head_pitch_rad')
-
- self._nose_confidence_threshold = rospy.get_param('~nose_confidence_threshold')
+ super().__init__('nearest_face_following_orientation_replication_node')
+ self._simulation = self.declare_parameter('simulation', False).get_parameter_value().bool_value
+ self._control_frequency = self.declare_parameter('control_frequency', 30.0).get_parameter_value().double_value
+ self._torso_control_alpha = self.declare_parameter('torso_control_alpha', 0.2).get_parameter_value().double_value
+ self._head_control_alpha = self.declare_parameter('head_control_alpha', 0.1).get_parameter_value().double_value
+ self._head_control_pitch_up_alpha_gain = self.declare_parameter('head_control_pitch_up_alpha_gain', 2.0).get_parameter_value().double_value
+ self._min_head_roll = self.declare_parameter('min_head_roll_rad', -0.3).get_parameter_value().double_value
+ self._max_head_roll = self.declare_parameter('max_head_roll_rad', 0.3).get_parameter_value().double_value
+ self._min_head_pitch = self.declare_parameter('min_head_pitch_rad', -0.15).get_parameter_value().double_value
+ self._max_head_pitch = self.declare_parameter('max_head_pitch_rad', 0.3).get_parameter_value().double_value
+ self._nose_confidence_threshold = self.declare_parameter('nose_confidence_threshold', 0.4).get_parameter_value().double_value
self._target_lock = threading.Lock()
self._target_torso_yaw = None
self._target_head_roll = 0.0
self._target_head_pitch = 0.0
- self._movement_commands = MovementCommands(self._simulation, 'other')
+ self._movement_commands = MovementCommands(self, self._simulation, 'other')
+
+ self._tf_buffer = Buffer()
+ self._tf_listener = TransformListener(self._tf_buffer, self)
- self._tf_listener = tf.TransformListener()
- self._video_analysis_sub = rospy.Subscriber('video_analysis', VideoAnalysis, self._video_analysis_cb, queue_size=1)
- self._face_orientation_sub = rospy.Subscriber('face_orientation', String, self._face_orientation_cb, queue_size=1)
- self._face_orientation_peer_data_sub = rospy.Subscriber('face_orientation_peer_data', PeerData, self._face_orientation_cb, queue_size=1)
+ self._video_analysis_sub = self.create_subscription(VideoAnalysis, 'video_analysis', self._video_analysis_cb, 1)
+ self._face_orientation_sub = self.create_subscription(String, 'face_orientation', self._face_orientation_cb, 1)
+ self._face_orientation_peer_data_sub = self.create_subscription(PeerData, 'face_orientation_peer_data', self._face_orientation_cb, 1)
+
+ self._timer = self.create_timer(1 / self._control_frequency, self._timer_callback)
def _video_analysis_cb(self, msg):
if self._movement_commands.is_filtering_all_messages:
return
if not msg.contains_3d_positions:
- rospy.logerr('The video analysis must contain 3d positions.')
+ self.get_logger().error('The video analysis must contain 3d positions.')
return
- face_position = self._find_nearest_face_position(msg.objects, msg.header)
- if face_position is None:
- return
+ try:
+ face_position = self._find_nearest_face_position(msg.objects, msg.header)
+ if face_position is None:
+ return
- target_torso_yaw, _ = vector_to_angles(face_position)
- if math.isfinite(target_torso_yaw):
- with self._target_lock:
- self._target_torso_yaw = target_torso_yaw
+ target_torso_yaw, _ = vector_to_angles(face_position)
+ if math.isfinite(target_torso_yaw):
+ with self._target_lock:
+ self._target_torso_yaw = target_torso_yaw
+ except TransformException as e:
+ self.get_logger().error(f'Could not transform: {e}')
def _find_nearest_face_position(self, objects, header):
nose_points_3d = []
@@ -85,7 +100,8 @@ def _transform_point(self, point, header):
temp_in_point.point.y = point.y
temp_in_point.point.z = point.z
- base_link_point = self._tf_listener.transformPoint('/base_link', temp_in_point)
+ transform = self._tf_buffer.lookup_transform('base_link', header.frame_id, rclpy.time.Time.from_msg(header.stamp))
+ base_link_point = tf2_geometry_msgs.do_transform_point(temp_in_point, transform)
point.x = base_link_point.point.x
point.y = base_link_point.point.y
@@ -94,7 +110,7 @@ def _transform_point(self, point, header):
def _face_orientation_cb(self, msg):
fields = msg.data.split(',')
if len(fields) != 2:
- rospy.logerr('Invalid face orientation message')
+ self.get_logger().error('Invalid face orientation message')
return
roll = np.clip(float(fields[0]), self._min_head_roll, self._max_head_roll)
@@ -104,14 +120,12 @@ def _face_orientation_cb(self, msg):
self._target_head_roll = roll
self._target_head_pitch = pitch
- def run(self):
- while not rospy.is_shutdown():
- self._rate.sleep()
- if self._movement_commands.is_filtering_all_messages:
- continue
+ def _timer_callback(self):
+ if self._movement_commands.is_filtering_all_messages:
+ return
- self._update_torso()
- self._update_head()
+ self._update_torso()
+ self._update_head()
def _update_torso(self):
with self._target_lock:
@@ -146,15 +160,24 @@ def _update_head(self):
self._movement_commands.move_head([0, 0, HEAD_ZERO_Z, roll, pitch, 0])
+ def run(self):
+ executor = rclpy.executors.MultiThreadedExecutor(num_threads=2)
+ executor.add_node(self)
+ executor.spin()
def main():
- rospy.init_node('nearest_face_following_orientation_replication_node')
+ rclpy.init()
nearest_face_following_orientation_replication_node = NearestFaceFollowingOrientationReplicationNode()
- nearest_face_following_orientation_replication_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ nearest_face_following_orientation_replication_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ nearest_face_following_orientation_replication_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/demos/connect4/scripts/nearest_face_orientation_node.py b/ros/demos/connect4/scripts/nearest_face_orientation_node.py
index 876464f0..03260136 100755
--- a/ros/demos/connect4/scripts/nearest_face_orientation_node.py
+++ b/ros/demos/connect4/scripts/nearest_face_orientation_node.py
@@ -5,10 +5,16 @@
import numpy as np
from scipy.spatial.transform import Rotation as R
-import rospy
-import tf
+import rclpy
+import rclpy.node
+
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+import tf2_geometry_msgs
+
from geometry_msgs.msg import PointStamped, Point, Vector3
-from video_analyzer.msg import VideoAnalysis
+from perception_msgs.msg import VideoAnalysis
from std_msgs.msg import String
from t_top import vector_to_angles
@@ -19,30 +25,36 @@
PERSON_POSE_RIGHT_EYE_INDEX = 2
-class NearestFaceOrientationNode:
+class NearestFaceOrientationNode(rclpy.node.Node):
def __init__(self):
- self._nose_confidence_threshold = rospy.get_param('~nose_confidence_threshold')
- self._pitch_offset_rad = rospy.get_param('~pitch_offset_rad')
- self._filter_alpha = rospy.get_param('~filter_alpha')
- self._roll_dead_zone = rospy.get_param('~roll_dead_zone')
+ super().__init__('nearest_face_orientation_node')
+
+ self._nose_confidence_threshold = self.declare_parameter('nose_confidence_threshold', 0.4).get_parameter_value().double_value
+ self._pitch_offset_rad = self.declare_parameter('pitch_offset_rad', -0.8).get_parameter_value().double_value
+ self._filter_alpha = self.declare_parameter('filter_alpha', 0.65).get_parameter_value().double_value
+ self._roll_dead_zone = self.declare_parameter('roll_dead_zone', 0.05).get_parameter_value().double_value
self._roll = 0.0
self._pitch = 0.0
- self._tf_listener = tf.TransformListener()
+ self._tf_buffer = Buffer()
+ self._tf_listener = TransformListener(self._tf_buffer, self)
- self._face_orientation_pub = rospy.Publisher('face_orientation', String, queue_size=1)
- self._video_analysis_sub = rospy.Subscriber('video_analysis', VideoAnalysis, self._video_analysis_cb, queue_size=1)
+ self._face_orientation_pub = self.create_publisher(String, 'face_orientation', 1)
+ self._video_analysis_sub = self.create_subscription(VideoAnalysis, 'video_analysis', self._video_analysis_cb, 1)
def _video_analysis_cb(self, msg):
if not msg.contains_3d_positions:
- rospy.logerr('The video analysis must contain 3d positions.')
+ self.get_logger().error('The video analysis must contain 3d positions.')
return
- self._update_nearest_face_orientation(msg.objects, msg.header)
- msg = String()
- msg.data = f'{self._roll},{self._pitch}'
- self._face_orientation_pub.publish(msg)
+ try:
+ self._update_nearest_face_orientation(msg.objects, msg.header)
+ msg = String()
+ msg.data = f'{self._roll},{self._pitch}'
+ self._face_orientation_pub.publish(msg)
+ except TransformException as e:
+ self.get_logger().error(f'Could not transform: {e}')
def _update_nearest_face_orientation(self, objects, header):
poses = []
@@ -97,7 +109,8 @@ def _transform_point(self, point, header):
temp_in_point.point.y = point.y
temp_in_point.point.z = point.z
- stewart_base_point = self._tf_listener.transformPoint('/stewart_base', temp_in_point)
+ transform = self._tf_buffer.lookup_transform('stewart_base', header.frame_id, rclpy.time.Time.from_msg(header.stamp))
+ stewart_base_point = tf2_geometry_msgs.do_transform_point(temp_in_point, transform)
point.x = stewart_base_point.point.x
point.y = stewart_base_point.point.y
@@ -113,7 +126,7 @@ def _vector_from_to(self, from_p, to_p):
def _vector_to_yaw(self, vector):
vector = np.array([vector.x, vector.y, vector.z])
vector /= np.linalg.norm(vector)
-
+
return np.arctan2(vector[0], vector[1])
def _rotate_point_yaw(self, point, yaw):
@@ -141,17 +154,22 @@ def _vector_to_roll_pitch(self, vector):
return roll, pitch
def run(self):
- rospy.spin()
+ rclpy.spin(self)
def main():
- rospy.init_node('nearest_face_orientation_node')
+ rclpy.init()
nearest_face_orientation_node = NearestFaceOrientationNode()
- nearest_face_orientation_node.run()
-
-if __name__ == '__main__':
try:
- main()
- except rospy.ROSInterruptException:
+ nearest_face_orientation_node.run()
+ except KeyboardInterrupt:
pass
+ finally:
+ nearest_face_orientation_node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros/demos/connect4/src/Connect4Widget.cpp b/ros/demos/connect4/src/Connect4Widget.cpp
index 52af1ec1..de74658e 100644
--- a/ros/demos/connect4/src/Connect4Widget.cpp
+++ b/ros/demos/connect4/src/Connect4Widget.cpp
@@ -2,7 +2,6 @@
#include "QtUtils.h"
#include
-#include
#include
#include
@@ -14,12 +13,11 @@
constexpr float ENABLED_VOLUME = 1;
constexpr float DISABLED_VOLUME = 0;
constexpr int SET_VOLUME_TIMER_INTERVAL_MS = 500;
-
constexpr int WEB_SOCKET_STATUS_TIMEOUT_MS = 1000;
-Connect4Widget::Connect4Widget(ros::NodeHandle& nodeHandle, std::shared_ptr desireSet, QWidget* parent)
- : m_nodeHandle(nodeHandle),
+Connect4Widget::Connect4Widget(rclcpp::Node::SharedPtr node, std::shared_ptr desireSet, QWidget* parent)
+ : m_node(std::move(node)),
m_desireSet(std::move(desireSet)),
m_enabled(false),
m_connect4ManagerConnectionRequested(false)
@@ -29,20 +27,33 @@ Connect4Widget::Connect4Widget(ros::NodeHandle& nodeHandle, std::shared_ptraddWidget(m_imageDisplay);
setLayout(layout);
- m_startButtonPressedSub =
- m_nodeHandle.subscribe("daemon/start_button_pressed", 1, &Connect4Widget::startButtonPressedCallback, this);
- m_stopButtonPressedSub =
- m_nodeHandle.subscribe("daemon/stop_button_pressed", 1, &Connect4Widget::stopButtonPressedCallback, this);
- m_remoteImageSub = m_nodeHandle.subscribe("/webrtc_image", 1, &Connect4Widget::remoteImageCallback, this);
+ m_startButtonPressedSub = m_node->create_subscription(
+ "daemon/start_button_pressed",
+ 1,
+ [this](const std_msgs::msg::Empty::SharedPtr msg) { startButtonPressedCallback(msg); });
+ m_stopButtonPressedSub = m_node->create_subscription(
+ "daemon/stop_button_pressed",
+ 1,
+ [this](const std_msgs::msg::Empty::SharedPtr msg) { stopButtonPressedCallback(msg); });
+
+ m_remoteImageSub = m_node->create_subscription(
+ "webrtc_image",
+ 1,
+ [this](const opentera_webrtc_ros_msgs::msg::PeerImage::SharedPtr msg) { remoteImageCallback(msg); });
+
+ m_openteraEventSubscriber = m_node->create_subscription(
+ "events",
+ 1,
+ [this](const opentera_webrtc_ros_msgs::msg::OpenTeraEvent::SharedPtr msg) { openteraEventCallback(msg); });
+
- m_volumePub = m_nodeHandle.advertise("volume", 1);
+
+ m_volumePub = m_node->create_publisher("volume", 1);
m_setVolumeTimer = new QTimer(this);
connect(m_setVolumeTimer, &QTimer::timeout, this, &Connect4Widget::onSetVolumeTimerTimeout);
m_setVolumeTimer->start(SET_VOLUME_TIMER_INTERVAL_MS);
- m_openteraEventSubscriber = m_nodeHandle.subscribe("events", 10, &Connect4Widget::openteraEventCallback, this);
-
m_connect4ManagerWebSocketTimer = new QTimer(this);
connect(m_connect4ManagerWebSocketTimer, &QTimer::timeout, this, &Connect4Widget::onConnect4ManagerWebSocketTimeout);
m_connect4ManagerWebSocketTimer->start(WEB_SOCKET_STATUS_TIMEOUT_MS);
@@ -91,12 +102,12 @@ void Connect4Widget::onConnect4ManagerWebSocketSslErrors(const QList&
{
errorString += error.errorString() + ", ";
}
- ROS_ERROR_STREAM("Connect 4 manager web socket SSL errors: " << errorString.toStdString());
+ RCLCPP_ERROR_STREAM(m_node->get_logger(), "Connect 4 manager web socket SSL errors: " << errorString.toStdString());
}
void Connect4Widget::onConnect4ManagerWebSocketConnected()
{
- ROS_INFO("Connect 4 manager web socket connected");
+ RCLCPP_INFO(m_node->get_logger(), "Connect 4 manager web socket connected");
QJsonObject dataObject;
dataObject.insert("participant_name", m_observedParticipantName);
@@ -105,24 +116,25 @@ void Connect4Widget::onConnect4ManagerWebSocketConnected()
void Connect4Widget::onConnect4ManagerWebSocketDisconnected()
{
- ROS_INFO("Connect 4 manager web socket disconnected");
+ RCLCPP_INFO(m_node->get_logger(), "Connect 4 manager web socket disconnected");
}
void Connect4Widget::onConnect4ManagerWebSocketErrorOccurred(QAbstractSocket::SocketError error)
{
- ROS_ERROR_STREAM("Connect 4 manager web socket error: " <<
+ RCLCPP_ERROR_STREAM(m_node->get_logger(), "Connect 4 manager web socket error: " <<
QMetaEnum::fromType().valueToKey(error));
}
void Connect4Widget::onConnect4ManagerWebSocketTextMessageReceived(const QString& message)
{
- ROS_INFO_STREAM("Connect 4 manager web socket message: " << message.toStdString());
+ RCLCPP_INFO_STREAM(m_node->get_logger(), "Connect 4 manager web socket message: " << message.toStdString());
QJsonParseError jsonParseError;
QJsonDocument jsonMessage = QJsonDocument::fromJson(message.toUtf8(), &jsonParseError);
if (jsonParseError.error != QJsonParseError::NoError)
{
- ROS_ERROR_STREAM("onnect 4 manager web socket message parsing error:" << jsonParseError.errorString().toStdString());
+ RCLCPP_ERROR_STREAM(m_node->get_logger(), "Connect 4 manager web socket message parsing error: " <<
+ jsonParseError.errorString().toStdString());
return;
}
@@ -132,7 +144,7 @@ void Connect4Widget::onConnect4ManagerWebSocketTextMessageReceived(const QString
}
}
-void Connect4Widget::startButtonPressedCallback(const std_msgs::EmptyConstPtr& msg)
+void Connect4Widget::startButtonPressedCallback(const std_msgs::msg::Empty::SharedPtr msg)
{
m_enabled = true;
setVolume(ENABLED_VOLUME);
@@ -141,7 +153,7 @@ void Connect4Widget::startButtonPressedCallback(const std_msgs::EmptyConstPtr& m
m_desireSet->addDesire();
}
-void Connect4Widget::stopButtonPressedCallback(const std_msgs::EmptyConstPtr& msg)
+void Connect4Widget::stopButtonPressedCallback(const std_msgs::msg::Empty::SharedPtr msg)
{
m_enabled = false;
setVolume(DISABLED_VOLUME);
@@ -152,11 +164,11 @@ void Connect4Widget::stopButtonPressedCallback(const std_msgs::EmptyConstPtr& ms
invokeLater([this]() { m_imageDisplay->setImage(QImage()); });
}
-void Connect4Widget::remoteImageCallback(const opentera_webrtc_ros_msgs::PeerImageConstPtr& msg)
+void Connect4Widget::remoteImageCallback(const opentera_webrtc_ros_msgs::msg::PeerImage::SharedPtr msg)
{
if (msg->frame.encoding != "bgr8")
{
- ROS_ERROR("Not support image encoding (Connect4Widget::remoteImageCallback).");
+ RCLCPP_ERROR(m_node->get_logger(), "Not support image encoding (Connect4Widget::remoteImageCallback).");
return;
}
@@ -173,12 +185,12 @@ void Connect4Widget::remoteImageCallback(const opentera_webrtc_ros_msgs::PeerIma
void Connect4Widget::setVolume(float volume)
{
- std_msgs::Float32 msg;
+ std_msgs::msg::Float32 msg;
msg.data = volume;
- m_volumePub.publish(msg);
+ m_volumePub->publish(msg);
}
-void Connect4Widget::openteraEventCallback(const opentera_webrtc_ros_msgs::OpenTeraEventConstPtr& msg)
+void Connect4Widget::openteraEventCallback(const opentera_webrtc_ros_msgs::msg::OpenTeraEvent::SharedPtr msg)
{
if (!msg->join_session_events.empty())
{
@@ -194,7 +206,7 @@ void Connect4Widget::openteraEventCallback(const opentera_webrtc_ros_msgs::OpenT
parseSessionUrl(sessionUrl, m_connect4ManagerWebSocketUrl, m_connect4ManagerWebSocketPassword);
m_observedParticipantName = getParticipantName(deviceName, sessionParameters);
- ROS_INFO_STREAM("Connect4 Manager Web Socket: Connection (sessionUrl=" << sessionUrl <<
+ RCLCPP_INFO_STREAM(m_node->get_logger(), "Connect4 Manager Web Socket: Connection (sessionUrl=" << sessionUrl <<
", webSocketUrl=" << m_connect4ManagerWebSocketUrl.toStdString() <<
", password=" << m_connect4ManagerWebSocketPassword.toStdString() <<
", deviceName=" << deviceName <<
@@ -242,7 +254,8 @@ QString Connect4Widget::getParticipantName(const std::string& deviceName, const
if (jsonParseError.error != QJsonParseError::NoError)
{
- ROS_ERROR_STREAM("Connect4 Game Data Channel: getParticipantName error: " << jsonParseError.errorString().toStdString());
+ RCLCPP_ERROR_STREAM(m_node->get_logger(), "Connect4 Game Data Channel: getParticipantName error: " <<
+ jsonParseError.errorString().toStdString());
return "";
}
@@ -251,7 +264,7 @@ QString Connect4Widget::getParticipantName(const std::string& deviceName, const
QString participant2 = jsonObject["participant2"].toString();
QString robot1 = jsonObject["robot1"].toString();
QString robot2 = jsonObject["robot2"].toString();
-
+
QString qDeviceName(deviceName.c_str());
if (qDeviceName == robot1)
{
@@ -263,7 +276,7 @@ QString Connect4Widget::getParticipantName(const std::string& deviceName, const
}
else
{
- ROS_ERROR("Connect4 Game Data Channel: Device name not found");
+ RCLCPP_ERROR(m_node->get_logger(), "Connect4 Game Data Channel: Device name not found");
return "";
}
}
@@ -307,14 +320,14 @@ void Connect4Widget::addRotatingSinDesire(uint8_t r, uint8_t g, uint8_t b)
constexpr double SPEED = 2.0;
constexpr double DURATION_S = 4.0;
- daemon_ros_client::LedColor c;
+ daemon_ros_client::msg::LedColor c;
c.red = r;
c.green = g;
c.blue = b;
m_desireSet->addDesire(
"rotating_sin",
- std::vector{c},
+ std::vector{c},
SPEED,
DURATION_S);
}
diff --git a/ros/demos/connect4/src/Connect4Widget.h b/ros/demos/connect4/src/Connect4Widget.h
index 4ca71ec1..35be5d01 100644
--- a/ros/demos/connect4/src/Connect4Widget.h
+++ b/ros/demos/connect4/src/Connect4Widget.h
@@ -8,11 +8,12 @@
#include
#include
-#include
+#include
#include
-#include
-#include
-#include
+#include
+#include
+#include
+#include
#include
#include
@@ -21,19 +22,21 @@ class Connect4Widget : public QWidget
{
Q_OBJECT
- ros::NodeHandle& m_nodeHandle;
+ rclcpp::Node::SharedPtr m_node;
std::shared_ptr m_desireSet;
std::atomic_bool m_enabled;
- ros::Subscriber m_startButtonPressedSub;
- ros::Subscriber m_stopButtonPressedSub;
- ros::Subscriber m_remoteImageSub;
+ rclcpp::Subscription::SharedPtr m_startButtonPressedSub;
+ rclcpp::Subscription::SharedPtr m_stopButtonPressedSub;
+ rclcpp::Subscription::SharedPtr m_remoteImageSub;
+ rclcpp::Subscription::SharedPtr m_openteraEventSubscriber;
+
+ rclcpp::Publisher::SharedPtr m_volumePub;
- ros::Publisher m_volumePub;
QTimer* m_setVolumeTimer;
- ros::Subscriber m_openteraEventSubscriber;
+
QString m_connect4ManagerWebSocketUrl;
QString m_connect4ManagerWebSocketPassword;
QString m_observedParticipantName;
@@ -43,7 +46,7 @@ class Connect4Widget : public QWidget
QWebSocket* m_connect4ManagerWebSocket;
public:
- Connect4Widget(ros::NodeHandle& nodeHandle, std::shared_ptr desireSet, QWidget* parent = nullptr);
+ Connect4Widget(rclcpp::Node::SharedPtr node, std::shared_ptr desireSet, QWidget* parent = nullptr);
private Q_SLOTS:
void onSetVolumeTimerTimeout();
@@ -57,13 +60,13 @@ private Q_SLOTS:
void onConnect4ManagerWebSocketTextMessageReceived(const QString& message);
private:
- void startButtonPressedCallback(const std_msgs::EmptyConstPtr& msg);
- void stopButtonPressedCallback(const std_msgs::EmptyConstPtr& msg);
- void remoteImageCallback(const opentera_webrtc_ros_msgs::PeerImageConstPtr& msg);
+ void startButtonPressedCallback(const std_msgs::msg::Empty::SharedPtr msg);
+ void stopButtonPressedCallback(const std_msgs::msg::Empty::SharedPtr msg);
+ void remoteImageCallback(const opentera_webrtc_ros_msgs::msg::PeerImage::SharedPtr msg);
void setVolume(float volume);
- void openteraEventCallback(const opentera_webrtc_ros_msgs::OpenTeraEventConstPtr& msg);
+ void openteraEventCallback(const opentera_webrtc_ros_msgs::msg::OpenTeraEvent::SharedPtr msg);
bool sendConnect4ManagerEvent(const QString& event, const QJsonObject& data);
QString getParticipantName(const std::string& deviceName, const std::string& sessionParameters);
diff --git a/ros/demos/connect4/src/connect4_node.cpp b/ros/demos/connect4/src/connect4_node.cpp
index 571058a1..63c8eee5 100644
--- a/ros/demos/connect4/src/connect4_node.cpp
+++ b/ros/demos/connect4/src/connect4_node.cpp
@@ -2,7 +2,7 @@
#include
-#include
+#include
#include
#include
@@ -15,23 +15,21 @@
using namespace std;
constexpr bool WAIT_FOR_SERVICE = true;
+constexpr const char* NODE_NAME = "connnect4_node";
int startNode(int argc, char* argv[])
{
- ros::init(argc, argv, "control_panel_node");
- ros::NodeHandle nodeHandle;
- ros::NodeHandle privateNodeHandle("~");
+ auto node = rclcpp::Node::make_shared(NODE_NAME);
- bool fullscreen = false;
- privateNodeHandle.param("fullscreen", fullscreen, false);
+ bool fullscreen = node->declare_parameter("fullscreen", false);
auto desireSet = make_shared();
- auto filterPool = make_shared(nodeHandle, WAIT_FOR_SERVICE);
+ auto filterPool = make_shared(node, WAIT_FOR_SERVICE);
vector> strategies;
strategies.emplace_back(createTelepresenceStrategy(filterPool));
strategies.emplace_back(createNearestFaceFollowingStrategy(filterPool));
- strategies.emplace_back(createLedAnimationStrategy(filterPool, desireSet, nodeHandle));
+ strategies.emplace_back(createLedAnimationStrategy(filterPool, desireSet, node));
strategies.emplace_back(createCamera3dRecordingStrategy(filterPool));
@@ -40,7 +38,7 @@ int startNode(int argc, char* argv[])
HbbaLite hbba(desireSet, move(strategies), {{"sound", 1}}, move(solver));
QApplication application(argc, argv);
- Connect4Widget connect4Widget(nodeHandle, desireSet);
+ Connect4Widget connect4Widget(node, desireSet);
if (fullscreen)
{
connect4Widget.setWindowState(Qt::WindowFullScreen);
@@ -51,26 +49,33 @@ int startNode(int argc, char* argv[])
}
connect4Widget.show();
- ros::AsyncSpinner spinner(1);
- spinner.start();
+ // Run ROS in background
+ rclcpp::executors::SingleThreadedExecutor rosExecutor;
+ rosExecutor.add_node(node);
+ std::thread spinThread([&rosExecutor]() { rosExecutor.spin(); });
desireSet->addDesire();
int returnCode = application.exec();
- spinner.stop();
+ rosExecutor.cancel();
+ spinThread.join();
return returnCode;
}
int main(int argc, char* argv[])
{
+ rclcpp::init(argc, argv);
+
try
{
return startNode(argc, argv);
}
catch (const std::exception& e)
{
- ROS_ERROR_STREAM("Control panel crashed (" << e.what() << ")");
+ RCLCPP_ERROR_STREAM(rclcpp::get_logger(NODE_NAME), "Control panel crashed (" << e.what() << ")");
return -1;
}
+
+ rclcpp::shutdown();
}
diff --git a/ros/demos/control_panel/CMakeLists.txt b/ros/demos/control_panel/CMakeLists.txt
index c33627c7..56897513 100644
--- a/ros/demos/control_panel/CMakeLists.txt
+++ b/ros/demos/control_panel/CMakeLists.txt
@@ -1,153 +1,32 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
project(control_panel)
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-## 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
- audio_analyzer
- audio_utils
- gesture
- person_identification
- roscpp
- std_msgs
- sensor_msgs
- talk
- video_analyzer
- hbba_lite
- t_top_hbba_lite
- speech_to_text
- led_animations
- daemon_ros_client
-)
-
-## System dependencies are found with CMake's conventions
-
-set(control_panel_components Core Widgets Gui OpenGL WebKitWidgets)
-find_package(Qt5 COMPONENTS ${control_panel_components} REQUIRED)
-
-
-## 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 ##
-################################################
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+ set(CMAKE_C_STANDARD 99)
+endif()
-## 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 tag for "message_generation"
-## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-## but can be declared for certainty nonetheless:
-## * add a exec_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 ...)
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
-## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
-# )
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
-## Generate services in the 'srv' folder
-# add_service_files(
-# FILES
-# Service1.srv
-# Service2.srv
-# )
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(perception_msgs REQUIRED)
+find_package(t_top_hbba_lite REQUIRED)
+find_package(rosidl_typesupport_cpp REQUIRED)
-## 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
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-## * add "dynamic_reconfigure" to
-## find_package(catkin REQUIRED COMPONENTS ...)
-## * uncomment the "generate_dynamic_reconfigure_options" section below
-## and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-# cfg/DynReconf1.cfg
-# cfg/DynReconf2.cfg
-# )
-
-###################################
-## 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 your 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 control_panel
- # CATKIN_DEPENDS audio_analyzer gesture person_identification roscpp std_msgs talk video_analyzer
- # DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
- include
- ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-# src/${PROJECT_NAME}/control_panel.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/control_panel_node.cpp)
+set(control_panel_components Core Widgets Gui OpenGL WebKitWidgets)
+find_package(Qt5 COMPONENTS ${control_panel_components} REQUIRED)
+# C++ Nodes
set(moc_headers
src/widgets/ImageDisplay.h
src/widgets/ControlPanel.h
@@ -179,74 +58,14 @@ qt5_wrap_cpp(project_moc_srcs ${moc_headers})
add_executable(${PROJECT_NAME}_node ${srcs} ${uis} ${moc_headers} ${other_headers} ${project_moc_uis} ${project_moc_srcs})
qt5_use_modules(${PROJECT_NAME}_node ${control_panel_components})
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}_node
- ${catkin_LIBRARIES}
- ${control_panel_components_link}
+ament_target_dependencies(${PROJECT_NAME}_node
+ rclcpp
+ t_top_hbba_lite
+ perception_msgs
)
+install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME})
-#############
-## 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
-# catkin_install_python(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_control_panel.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
+# Launch files
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ament_package()
diff --git a/ros/demos/control_panel/launch/control_panel.launch b/ros/demos/control_panel/launch/control_panel.launch.xml
similarity index 51%
rename from ros/demos/control_panel/launch/control_panel.launch
rename to ros/demos/control_panel/launch/control_panel.launch.xml
index 9b98984d..a8e0c165 100644
--- a/ros/demos/control_panel/launch/control_panel.launch
+++ b/ros/demos/control_panel/launch/control_panel.launch.xml
@@ -2,13 +2,13 @@
-
-
+
+
-
-
-
+
+
+
diff --git a/ros/demos/control_panel/package.xml b/ros/demos/control_panel/package.xml
index 1079ce60..014f4f46 100644
--- a/ros/demos/control_panel/package.xml
+++ b/ros/demos/control_panel/package.xml
@@ -1,101 +1,26 @@
-
+
+
control_panel
0.0.0
The control_panel package
+ Marc-Antoine Maheux
+ GPL-3.0 license
-
-
-
- marc-antoine
+ ament_cmake
+ rclcpp
+ audio_utils_msgs
+ t_top_hbba_lite
+ perception_msgs
-
-
-
- TODO
+ rosidl_default_runtime
+ rosidl_interface_packages
+ ament_lint_auto
+ ament_lint_common
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- catkin
- audio_analyzer
- audio_utils
- gesture
- person_identification
- roscpp
- std_msgs
- talk
- video_analyzer
- sensor_msgs
- hbba_lite
- t_top_hbba_lite
- speech_to_text
- led_animations
- daemon_ros_client
- audio_analyzer
- audio_utils
- gesture
- person_identification
- roscpp
- std_msgs
- talk
- video_analyzer
- sensor_msgs
- hbba_lite
- t_top_hbba_lite
- speech_to_text
- led_animations
- daemon_ros_client
- audio_analyzer
- audio_utils
- gesture
- person_identification
- roscpp
- std_msgs
- talk
- video_analyzer
- sensor_msgs
- hbba_lite
- t_top_hbba_lite
- speech_to_text
- led_animations
- daemon_ros_client
-
-
-
-
-
+ ament_cmake
diff --git a/ros/demos/control_panel/src/control_panel_node.cpp b/ros/demos/control_panel/src/control_panel_node.cpp
index 957d5e4b..e0f61ebd 100644
--- a/ros/demos/control_panel/src/control_panel_node.cpp
+++ b/ros/demos/control_panel/src/control_panel_node.cpp
@@ -2,7 +2,7 @@
#include
-#include
+#include
#include
#include
@@ -17,19 +17,17 @@
using namespace std;
constexpr bool WAIT_FOR_SERVICE = true;
+constexpr const char* NODE_NAME = "control_panel_node";
int startNode(int argc, char* argv[])
{
- ros::init(argc, argv, "control_panel_node");
- ros::NodeHandle nodeHandle;
- ros::NodeHandle privateNodeHandle("~");
+ auto node = rclcpp::Node::make_shared(NODE_NAME);
- bool camera2dWideEnabled = false;
- privateNodeHandle.param("camera_2d_wide_enabled", camera2dWideEnabled, false);
+ bool camera2dWideEnabled = node->declare_parameter("camera_2d_wide_enabled", false);
auto desireSet = make_shared();
- auto rosFilterPool = make_unique(nodeHandle, WAIT_FOR_SERVICE);
- auto filterPool = make_shared(move(rosFilterPool));
+ auto rosFilterPool = make_unique(node, WAIT_FOR_SERVICE);
+ auto filterPool = make_shared(node, move(rosFilterPool));
vector> strategies;
strategies.emplace_back(createRobotNameDetectorStrategy(filterPool));
@@ -37,17 +35,16 @@ int startNode(int argc, char* argv[])
strategies.emplace_back(createAudioAnalyzerStrategy(filterPool));
strategies.emplace_back(createVadStrategy(filterPool));
strategies.emplace_back(createSpeechToTextStrategy(filterPool));
-
strategies.emplace_back(createExploreStrategy(filterPool));
- strategies.emplace_back(createFaceAnimationStrategy(filterPool, nodeHandle));
- strategies.emplace_back(createLedEmotionStrategy(filterPool, nodeHandle));
- strategies.emplace_back(createLedAnimationStrategy(filterPool, desireSet, nodeHandle));
+ strategies.emplace_back(createFaceAnimationStrategy(filterPool, node));
+ strategies.emplace_back(createLedEmotionStrategy(filterPool, node));
+ strategies.emplace_back(createLedAnimationStrategy(filterPool, desireSet, node));
strategies.emplace_back(createSoundFollowingStrategy(filterPool));
strategies.emplace_back(createNearestFaceFollowingStrategy(filterPool));
- strategies.emplace_back(createSpecificFaceFollowingStrategy(filterPool, nodeHandle));
+ strategies.emplace_back(createSpecificFaceFollowingStrategy(filterPool, node));
strategies.emplace_back(createSoundObjectPersonFollowingStrategy(filterPool));
- strategies.emplace_back(createTalkStrategy(filterPool, desireSet, nodeHandle));
- strategies.emplace_back(createGestureStrategy(filterPool, desireSet, nodeHandle));
+ strategies.emplace_back(createTalkStrategy(filterPool, desireSet, node));
+ strategies.emplace_back(createGestureStrategy(filterPool, desireSet, node));
strategies.emplace_back(createDanceStrategy(filterPool));
strategies.emplace_back(createTooCloseReactionStrategy(filterPool));
@@ -57,31 +54,40 @@ int startNode(int argc, char* argv[])
}
auto solver = make_unique();
- auto strategyStateLogger = make_unique();
+ auto strategyStateLogger = make_unique(node);
HbbaLite hbba(desireSet, move(strategies), {{"sound", 1}}, move(solver), move(strategyStateLogger));
QApplication application(argc, argv);
- ControlPanel controlPanel(nodeHandle, desireSet, camera2dWideEnabled);
+ ControlPanel controlPanel(node, desireSet, camera2dWideEnabled);
// controlPanel.setFixedSize(600, 900);
controlPanel.show();
- ros::AsyncSpinner spinner(1);
- spinner.start();
+ // Run ROS in background
+ rclcpp::executors::SingleThreadedExecutor rosExecutor;
+ rosExecutor.add_node(node);
+ std::thread spinThread([&rosExecutor]() { rosExecutor.spin(); });
+
int returnCode = application.exec();
- spinner.stop();
+
+ rosExecutor.cancel();
+ spinThread.join();
return returnCode;
}
int main(int argc, char* argv[])
{
+ rclcpp::init(argc, argv);
+
try
{
return startNode(argc, argv);
}
catch (const std::exception& e)
{
- ROS_ERROR_STREAM("Control panel crashed (" << e.what() << ")");
+ RCLCPP_ERROR_STREAM(rclcpp::get_logger(NODE_NAME), "Control panel crashed (" << e.what() << ")");
return -1;
}
+
+ rclcpp::shutdown();
}
diff --git a/ros/demos/control_panel/src/widgets/ControlPanel.cpp b/ros/demos/control_panel/src/widgets/ControlPanel.cpp
index 1bd8ca1a..0191387e 100644
--- a/ros/demos/control_panel/src/widgets/ControlPanel.cpp
+++ b/ros/demos/control_panel/src/widgets/ControlPanel.cpp
@@ -5,8 +5,6 @@
#include
#include
-#include
-
using namespace std;
constexpr int AUDIO_POWER_AMPLIFIER_MIN_VOLUME = 0;
@@ -14,30 +12,32 @@ constexpr int AUDIO_POWER_AMPLIFIER_MAX_VOLUME = 63;
constexpr int AUDIO_POWER_AMPLIFIER_DEFAULT_VOLUME = 24;
ControlPanel::ControlPanel(
- ros::NodeHandle& nodeHandle,
+ rclcpp::Node::SharedPtr node,
shared_ptr desireSet,
bool camera2dWideEnabled,
QWidget* parent)
: QWidget(parent),
- m_nodeHandle(nodeHandle),
+ m_node(std::move(node)),
m_desireSet(std::move(desireSet))
{
- m_volumePublisher = nodeHandle.advertise("daemon/set_volume", 1);
+ m_volumePublisher = m_node->create_publisher("daemon/set_volume", 1);
createUi(camera2dWideEnabled);
- m_baseStatusSubscriber =
- nodeHandle.subscribe("daemon/base_status", 1, &ControlPanel::baseStatusSubscriberCallback, this);
+ m_baseStatusSubscriber = m_node->create_subscription(
+ "daemon/base_status",
+ 1,
+ [this](const daemon_ros_client::msg::BaseStatus::SharedPtr msg) { baseStatusSubscriberCallback(msg); });
}
void ControlPanel::onVolumeChanged(int volume)
{
- std_msgs::UInt8 msg;
+ std_msgs::msg::UInt8 msg;
msg.data = volume;
- m_volumePublisher.publish(msg);
+ m_volumePublisher->publish(msg);
}
-void ControlPanel::baseStatusSubscriberCallback(const daemon_ros_client::BaseStatus::ConstPtr& msg)
+void ControlPanel::baseStatusSubscriberCallback(const daemon_ros_client::msg::BaseStatus::SharedPtr msg)
{
int stateOfCharge = static_cast(msg->state_of_charge);
int volume = msg->volume;
@@ -59,11 +59,11 @@ void ControlPanel::createUi(bool camera2dWideEnabled)
setWindowTitle("Control Panel");
m_avatarTab = new AvatarTab(m_desireSet);
- m_speechTab = new SpeechTab(m_nodeHandle, m_desireSet);
- m_gestureTab = new GestureTab(m_nodeHandle, m_desireSet);
+ m_speechTab = new SpeechTab(m_node, m_desireSet);
+ m_gestureTab = new GestureTab(m_desireSet);
m_behaviorsTab = new BehaviorsTab(m_desireSet, camera2dWideEnabled);
- m_ledTab = new LedTab(m_nodeHandle, m_desireSet);
- m_perceptionsTab = new PerceptionsTab(m_nodeHandle, m_desireSet, camera2dWideEnabled);
+ m_ledTab = new LedTab(m_desireSet);
+ m_perceptionsTab = new PerceptionsTab(m_node, m_desireSet, camera2dWideEnabled);
m_tabWidget = new QTabWidget;
m_tabWidget->addTab(m_avatarTab, "Avatar");
diff --git a/ros/demos/control_panel/src/widgets/ControlPanel.h b/ros/demos/control_panel/src/widgets/ControlPanel.h
index 4be7b2cd..06997e11 100644
--- a/ros/demos/control_panel/src/widgets/ControlPanel.h
+++ b/ros/demos/control_panel/src/widgets/ControlPanel.h
@@ -12,9 +12,11 @@
#include
#include
-#include
-#include
-#include
+#include
+
+#include
+
+#include
#include
@@ -25,15 +27,16 @@ class ControlPanel : public QWidget
{
Q_OBJECT
- ros::NodeHandle& m_nodeHandle;
- ros::Publisher m_volumePublisher;
- ros::Subscriber m_baseStatusSubscriber;
+ rclcpp::Node::SharedPtr m_node;
+
+ rclcpp::Publisher::SharedPtr m_volumePublisher;
+ rclcpp::Subscription::SharedPtr m_baseStatusSubscriber;
std::shared_ptr m_desireSet;
public:
ControlPanel(
- ros::NodeHandle& nodeHandle,
+ rclcpp::Node::SharedPtr node,
std::shared_ptr desireSet,
bool camera2dWideEnabled,
QWidget* parent = nullptr);
@@ -42,7 +45,7 @@ private slots:
void onVolumeChanged(int volume);
private:
- void baseStatusSubscriberCallback(const daemon_ros_client::BaseStatus::ConstPtr& msg);
+ void baseStatusSubscriberCallback(const daemon_ros_client::msg::BaseStatus::SharedPtr msg);
void createUi(bool camera2dWideEnabled);
diff --git a/ros/demos/control_panel/src/widgets/GestureTab.cpp b/ros/demos/control_panel/src/widgets/GestureTab.cpp
index 7298ed52..1615ec68 100644
--- a/ros/demos/control_panel/src/widgets/GestureTab.cpp
+++ b/ros/demos/control_panel/src/widgets/GestureTab.cpp
@@ -7,9 +7,8 @@
using namespace std;
-GestureTab::GestureTab(ros::NodeHandle& nodeHandle, shared_ptr desireSet, QWidget* parent)
+GestureTab::GestureTab(shared_ptr desireSet, QWidget* parent)
: QWidget(parent),
- m_nodeHandle(nodeHandle),
m_desireSet(std::move(desireSet))
{
createUi();
@@ -50,7 +49,10 @@ void GestureTab::setEnabledAllButtons(bool enabled)
m_maybeButton->setEnabled(enabled);
m_originAllButton->setEnabled(enabled);
m_originHeadButton->setEnabled(enabled);
+ m_slowOriginHeadButton->setEnabled(enabled);
m_originTorsoButton->setEnabled(enabled);
+ m_thinkingButton->setEnabled(enabled);
+ m_sadButton->setEnabled(enabled);
}
void GestureTab::createUi()
@@ -70,16 +72,28 @@ void GestureTab::createUi()
m_originHeadButton = new QPushButton("Origin Head");
connect(m_originHeadButton, &QPushButton::clicked, this, [this]() { onGestureButtonClicked("origin_head"); });
+ m_slowOriginHeadButton = new QPushButton("Slow Origin Head");
+ connect(m_slowOriginHeadButton, &QPushButton::clicked, this, [this]() { onGestureButtonClicked("slow_origin_head"); });
+
m_originTorsoButton = new QPushButton("Origin Torso");
connect(m_originTorsoButton, &QPushButton::clicked, this, [this]() { onGestureButtonClicked("origin_torso"); });
+ m_thinkingButton = new QPushButton("Thinking");
+ connect(m_thinkingButton, &QPushButton::clicked, this, [this]() { onGestureButtonClicked("thinking"); });
+
+ m_sadButton = new QPushButton("Sad");
+ connect(m_sadButton, &QPushButton::clicked, this, [this]() { onGestureButtonClicked("sad"); });
+
auto globalLayout = new QVBoxLayout;
globalLayout->addWidget(m_yesButton);
globalLayout->addWidget(m_noButton);
globalLayout->addWidget(m_maybeButton);
globalLayout->addWidget(m_originAllButton);
globalLayout->addWidget(m_originHeadButton);
+ globalLayout->addWidget(m_slowOriginHeadButton);
globalLayout->addWidget(m_originTorsoButton);
+ globalLayout->addWidget(m_thinkingButton);
+ globalLayout->addWidget(m_sadButton);
globalLayout->addStretch();
setLayout(globalLayout);
diff --git a/ros/demos/control_panel/src/widgets/GestureTab.h b/ros/demos/control_panel/src/widgets/GestureTab.h
index d01f92e2..9dfc3f22 100644
--- a/ros/demos/control_panel/src/widgets/GestureTab.h
+++ b/ros/demos/control_panel/src/widgets/GestureTab.h
@@ -5,9 +5,6 @@
#include
#include
-#include
-#include
-
#include
#include
@@ -17,13 +14,11 @@ class GestureTab : public QWidget, public DesireSetObserver
{
Q_OBJECT
- ros::NodeHandle& m_nodeHandle;
-
std::shared_ptr m_desireSet;
QVariant m_gestureDesireId;
public:
- GestureTab(ros::NodeHandle& nodeHandle, std::shared_ptr desireSet, QWidget* parent = nullptr);
+ GestureTab(std::shared_ptr desireSet, QWidget* parent = nullptr);
~GestureTab() override;
void onDesireSetChanged(const std::vector>& _) override;
@@ -42,7 +37,10 @@ private slots:
QPushButton* m_maybeButton;
QPushButton* m_originAllButton;
QPushButton* m_originHeadButton;
+ QPushButton* m_slowOriginHeadButton;
QPushButton* m_originTorsoButton;
+ QPushButton* m_thinkingButton;
+ QPushButton* m_sadButton;
};
#endif
diff --git a/ros/demos/control_panel/src/widgets/LedTab.cpp b/ros/demos/control_panel/src/widgets/LedTab.cpp
index 506f0ae7..4aab28cb 100644
--- a/ros/demos/control_panel/src/widgets/LedTab.cpp
+++ b/ros/demos/control_panel/src/widgets/LedTab.cpp
@@ -9,9 +9,9 @@
using namespace std;
-daemon_ros_client::LedColor color(uint8_t r, uint8_t g, uint8_t b)
+daemon_ros_client::msg::LedColor color(uint8_t r, uint8_t g, uint8_t b)
{
- daemon_ros_client::LedColor c;
+ daemon_ros_client::msg::LedColor c;
c.red = r;
c.green = g;
c.blue = b;
@@ -19,10 +19,7 @@ daemon_ros_client::LedColor color(uint8_t r, uint8_t g, uint8_t b)
}
-LedTab::LedTab(ros::NodeHandle& nodeHandle, shared_ptr desireSet, QWidget* parent)
- : QWidget(parent),
- m_nodeHandle(nodeHandle),
- m_desireSet(std::move(desireSet))
+LedTab::LedTab(shared_ptr desireSet, QWidget* parent) : QWidget(parent), m_desireSet(std::move(desireSet))
{
createUi();
}
@@ -54,7 +51,7 @@ void LedTab::onConstantAnimationButtonToggled(bool checked)
uncheckOtherAnimationButtons(m_constantAnimationButton);
auto desire =
- make_unique("constant", vector{color(255, 255, 255)});
+ make_unique("constant", vector{color(255, 255, 255)});
m_ledAnimationDesireId = static_cast(desire->id());
m_desireSet->addDesire(std::move(desire));
}
@@ -72,7 +69,7 @@ void LedTab::onRotatingSinAnimationButtonToggled(bool checked)
uncheckOtherAnimationButtons(m_rotatingSinAnimationButton);
auto desire =
- make_unique("rotating_sin", vector{color(0, 255, 0)});
+ make_unique("rotating_sin", vector{color(0, 255, 0)});
m_ledAnimationDesireId = static_cast(desire->id());
m_desireSet->addDesire(std::move(desire));
}
diff --git a/ros/demos/control_panel/src/widgets/LedTab.h b/ros/demos/control_panel/src/widgets/LedTab.h
index 45481968..8bfa0ecd 100644
--- a/ros/demos/control_panel/src/widgets/LedTab.h
+++ b/ros/demos/control_panel/src/widgets/LedTab.h
@@ -5,8 +5,6 @@
#include
#include
-#include
-
#include
#include
@@ -16,14 +14,12 @@ class LedTab : public QWidget
{
Q_OBJECT
- ros::NodeHandle& m_nodeHandle;
-
std::shared_ptr m_desireSet;
QVariant m_ledEmotionDesireId;
QVariant m_ledAnimationDesireId;
public:
- LedTab(ros::NodeHandle& nodeHandle, std::shared_ptr desireSet, QWidget* parent = nullptr);
+ LedTab(std::shared_ptr desireSet, QWidget* parent = nullptr);
~LedTab() override;
private slots:
diff --git a/ros/demos/control_panel/src/widgets/PerceptionsTab.cpp b/ros/demos/control_panel/src/widgets/PerceptionsTab.cpp
index 591102c1..0c0d64b2 100644
--- a/ros/demos/control_panel/src/widgets/PerceptionsTab.cpp
+++ b/ros/demos/control_panel/src/widgets/PerceptionsTab.cpp
@@ -53,13 +53,15 @@ vector getSemanticSegmentationPaletteFromClassCount(size_t classCount)
return palette;
}
-QImage semanticSegmentationToImage(const video_analyzer::SemanticSegmentation& semanticSegmentation)
+QImage semanticSegmentationToImage(
+ rclcpp::Node& node,
+ const perception_msgs::msg::SemanticSegmentation& semanticSegmentation)
{
int width = semanticSegmentation.width;
int height = semanticSegmentation.height;
if (semanticSegmentation.class_indexes.size() != width * height)
{
- ROS_ERROR("Invalid semantic segmentation (class_indexes.size() != width * height)");
+ RCLCPP_ERROR(node.get_logger(), "Invalid semantic segmentation (class_indexes.size() != width * height)");
return QImage();
}
@@ -83,39 +85,46 @@ QImage semanticSegmentationToImage(const video_analyzer::SemanticSegmentation& s
}
PerceptionsTab::PerceptionsTab(
- ros::NodeHandle& nodeHandle,
+ rclcpp::Node::SharedPtr node,
shared_ptr desireSet,
bool camera2dWideEnabled,
QWidget* parent)
: QWidget(parent),
- m_nodeHandle(nodeHandle),
+ m_node(std::move(node)),
m_desireSet(std::move(desireSet)),
m_camera2dWideEnabled(camera2dWideEnabled)
{
createUi();
- m_analyzedImage3dSubscriber =
- nodeHandle.subscribe("camera_3d/analysed_image", 1, &PerceptionsTab::analyzedImage3dSubscriberCallback, this);
+ m_analyzedImage3dSubscriber = m_node->create_subscription(
+ "camera_3d/analysed_image",
+ 1,
+ [this](const sensor_msgs::msg::Image::SharedPtr msg) { analyzedImage3dSubscriberCallback(msg); });
if (m_camera2dWideEnabled)
{
- m_analyzedImage2dWideSubscriber = nodeHandle.subscribe(
+ m_analyzedImage2dWideSubscriber = m_node->create_subscription(
"camera_2d_wide/analysed_image",
1,
- &PerceptionsTab::analyzedImage2dWideSubscriberCallback,
- this);
- m_videoAnalysis2dWideSubscriber = nodeHandle.subscribe(
+ [this](const sensor_msgs::msg::Image::SharedPtr msg) { analyzedImage2dWideSubscriberCallback(msg); });
+ m_videoAnalysis2dWideSubscriber = m_node->create_subscription(
"camera_2d_wide/video_analysis",
1,
- &PerceptionsTab::videoAnalysis2dWideSubscriberCallback,
- this);
+ [this](const perception_msgs::msg::VideoAnalysis::SharedPtr msg)
+ { videoAnalysis2dWideSubscriberCallback(msg); });
}
- m_audioAnalysisSubscriber =
- nodeHandle.subscribe("audio_analysis", 1, &PerceptionsTab::audioAnalysisSubscriberCallback, this);
- m_robotNameDetectedSubscriber =
- nodeHandle.subscribe("robot_name_detected", 1, &PerceptionsTab::robotNameDetectedSubscriberCallback, this);
- m_personNamesSubscriber =
- nodeHandle.subscribe("person_names", 1, &PerceptionsTab::personNamesSubscriberCallback, this);
+ m_audioAnalysisSubscriber = m_node->create_subscription(
+ "audio_analysis",
+ 1,
+ [this](const perception_msgs::msg::AudioAnalysis::SharedPtr msg) { audioAnalysisSubscriberCallback(msg); });
+ m_robotNameDetectedSubscriber = m_node->create_subscription(
+ "robot_name_detected",
+ 1,
+ [this](const std_msgs::msg::Empty::SharedPtr msg) { robotNameDetectedSubscriberCallback(msg); });
+ m_personNamesSubscriber = m_node->create_subscription(
+ "person_names",
+ 1,
+ [this](const perception_msgs::msg::PersonNames::SharedPtr msg) { personNamesSubscriberCallback(msg); });
}
void PerceptionsTab::onVideoAnalyzer3dButtonToggled(bool checked)
@@ -138,7 +147,7 @@ void PerceptionsTab::onRobotNameDetectorButtonToggled(bool checked)
toggleDesire(checked, m_robotNameDetectorDesireId);
}
-void PerceptionsTab::analyzedImage3dSubscriberCallback(const sensor_msgs::Image::ConstPtr& msg)
+void PerceptionsTab::analyzedImage3dSubscriberCallback(const sensor_msgs::msg::Image::SharedPtr msg)
{
if (msg->encoding != "rgb8")
{
@@ -152,7 +161,7 @@ void PerceptionsTab::analyzedImage3dSubscriberCallback(const sensor_msgs::Image:
});
}
-void PerceptionsTab::analyzedImage2dWideSubscriberCallback(const sensor_msgs::Image::ConstPtr& msg)
+void PerceptionsTab::analyzedImage2dWideSubscriberCallback(const sensor_msgs::msg::Image::SharedPtr msg)
{
if (msg->encoding != "rgb8")
{
@@ -167,7 +176,7 @@ void PerceptionsTab::analyzedImage2dWideSubscriberCallback(const sensor_msgs::Im
});
}
-void PerceptionsTab::videoAnalysis2dWideSubscriberCallback(const video_analyzer::VideoAnalysis::ConstPtr& msg)
+void PerceptionsTab::videoAnalysis2dWideSubscriberCallback(const perception_msgs::msg::VideoAnalysis::SharedPtr msg)
{
if (msg->semantic_segmentation.empty())
{
@@ -178,23 +187,23 @@ void PerceptionsTab::videoAnalysis2dWideSubscriberCallback(const video_analyzer:
[this, msg]()
{
m_videoAnalyzer2dWideSegmentationcImageDisplay->setImage(
- semanticSegmentationToImage(msg->semantic_segmentation[0]));
+ semanticSegmentationToImage(*m_node, msg->semantic_segmentation[0]));
});
}
-void PerceptionsTab::audioAnalysisSubscriberCallback(const audio_analyzer::AudioAnalysis::ConstPtr& msg)
+void PerceptionsTab::audioAnalysisSubscriberCallback(const perception_msgs::msg::AudioAnalysis::SharedPtr msg)
{
QString classes = mergeStdStrings(msg->audio_classes);
invokeLater([this, classes]() { m_soundClassesLineEdit->setText(classes); });
}
-void PerceptionsTab::robotNameDetectedSubscriberCallback(const std_msgs::Empty::ConstPtr& msg)
+void PerceptionsTab::robotNameDetectedSubscriberCallback(const std_msgs::msg::Empty::SharedPtr msg)
{
auto currentTime = QDateTime::currentDateTime();
invokeLater([this, currentTime]() { m_robotNameDetectionTimeLineEdit->setText(currentTime.toString()); });
}
-void PerceptionsTab::personNamesSubscriberCallback(const person_identification::PersonNames::ConstPtr& msg)
+void PerceptionsTab::personNamesSubscriberCallback(const perception_msgs::msg::PersonNames::SharedPtr msg)
{
vector names;
names.reserve(msg->names.size());
@@ -203,7 +212,7 @@ void PerceptionsTab::personNamesSubscriberCallback(const person_identification::
msg->names.begin(),
msg->names.end(),
back_inserter(names),
- [](const person_identification::PersonName& name) { return name.name; });
+ [](const perception_msgs::msg::PersonName& name) { return name.name; });
QString mergedNames = mergeStdStrings(names);
invokeLater([this, mergedNames]() { m_identifiedPersonsLineEdit->setText(mergedNames); });
diff --git a/ros/demos/control_panel/src/widgets/PerceptionsTab.h b/ros/demos/control_panel/src/widgets/PerceptionsTab.h
index a3380842..4e5226ca 100644
--- a/ros/demos/control_panel/src/widgets/PerceptionsTab.h
+++ b/ros/demos/control_panel/src/widgets/PerceptionsTab.h
@@ -8,12 +8,13 @@
#include
#include
-#include
-#include
-#include
-#include
-#include
-#include
+#include
+
+#include
+#include
+#include
+#include
+#include
#include