diff --git a/Dockerfile b/Dockerfile index 0884077..0ee926c 100644 --- a/Dockerfile +++ b/Dockerfile @@ -30,7 +30,6 @@ ENV WS=/ws RUN mkdir -p $WS/src COPY ./ .$WS/src/rae-ros -RUN cp -R .$WS/src/rae-ros/assets/. /usr/share RUN rm -rf .$WS/src/rae-ros/assets RUN rm -rf .$WS/src/rae-ros/rae_gazebo diff --git a/README.md b/README.md index 9089899..6ccd85d 100644 --- a/README.md +++ b/README.md @@ -3,219 +3,8 @@ This repository contains rae [ROS](https://www.ros.org/) integration files. **Note** The RAE project is under active development so you can expect changes in the API and improvements in the performance in near future. Please report problems on Github issues as it's important for the development efforts. -### Setting up procedure - -#### SSH - -1. Connect via USB cable or wifi `rae-`, password `wifiwifi@` (See [rae getting started documentation](https://docs.luxonis.com/projects/hardware/en/latest/pages/rae/#getting-started)). -2. To use SHH without typing password each time - `ssh-copy-id root@192.168.11.1`. Also a easier solution for USB devices, you can append to `~/.ssh/config` the following, entering just with `ssh ku`: -``` -Host ku 192.168.197.55 - HostName 192.168.197.55 - User root - StrictHostKeyChecking no - UserKnownHostsFile /dev/null -``` -The easiest way for accessing the file on Keembay, is to use VScode with Remote Containers extension by Ctrl + Shift + P -> connect to remote host. - -3. Currently date resets after each startup to set current - ssh root@192.168.11.1 sudo date -s @`( date -u +"%s" )` - -4. If you want to run ROS packages while bypassing RobotHub it would be advised to stop RH agent before starting docker containers, otherwise you can easily run into conflicts as they would be competing for same hardware resources - `robothub-ctl stop`. **Keep in mind** that since `wpa_supplicant` is a subproccess of the RH agent, the WiFi connection will get killed along with the agent. To resolve this we recommend you manually setup the WiFi connection as done in [this guide](https://docs-beta.luxonis.com/deploy/connect-device/rae/?v=Advanced+%28manual%29). - - -#### Generating docker image - -You can download prebuilt images form [dockerhub](https://hub.docker.com/r/luxonis/rae-ros-robot/tags), in which case you can skip first 2 steps in guide below. We reccomend using image tagged as humble as all other images are generally experimental images. You can download docker image with: - -`docker pull luxonis/rae-ros-robot:humble` - -Downloading prebuilt images is reccomended if you are not planning to considerably change source code. - -1. Clone repository `git clone git@github.com:luxonis/rae-ros.git` -2. Build docker image `cd rae && docker buildx build --platform arm64 --build-arg USE_RVIZ=0 --build-arg SIM=0 --build-arg ROS_DISTRO=humble --build-arg CORE_NUM=10 -f Dockerfile --squash -t : --load . -` -3. Upload docker image to robot. Connect robot to your PC via USB so you can transfer image quicker. Note that currently space on the robot is limited, so you need to have 7-8 GB of free space in `/data` directory - `docker save : | ssh -C root@192.168.197.55 docker load` -4. SSH into robot and run docker image - `docker run -it --restart=unless-stopped -v /dev/:/dev/ -v /sys/:/sys/ --privileged --net=host :` -5. Search for docker container name with `docker ps` -6. Attach to the shell - `docker attach `, or if you want to create separate session `docker exec -it zsh -7. To launch robot hardware - `ros2 launch rae_bringup robot.launch.py`. This launches: - - Motor drivers and differential controller - - Camera driver, currently set up to provide Depth and streams from left & right camera. Note here that you have to calibrate cameras (see steps below). Currently a default calibration file is loaded. It's located in `rae_camera/config/cal.json`. To use one on the device or from other path, change `i_external_calibration_path` parameter in `rae_camera/config/camera.yaml` - - Depth image -> LaserScan conversion node used for SLAM -8. Launching whole stack - `ros2 launch rae_bringup bringup.launch.py`. It has following arguments used for enabling parts of the stack: - - `enable_slam_toolbox` (true) - - `enable_rosbridge` (false) - - `enable_rtabmap` (false) - - `enable_nav` (false) -Example launch with an argument - `ros2 launch rae_bringup bringup.launch.py enable_nav:=false` - - -#### Calibration - -Every shipped rae has already been factory calibrated, so this step is rarely needed. Besides the section below, [Calibration documentation](https://docs.luxonis.com/projects/hardware/en/latest/pages/guides/calibration/) is also a good source of information. - -With PC conecteted to RAE via SSH (RH agent use depthai_gate): - -```bash -robothub-ctl stop -``` -On your PC: -``` -git clone --branch rvc3_calibration https://github.com/luxonis/depthai.git -cd depthai/ -python3 install_requirements.py -# To calibrate rae's front cameras - for back cameras we would change the board name to "RAE-D-E" -python3 calibrate.py -s -brd RAE-A-B-C -cd 1 -c 3 -``` -Some tips: -1. Try to fill stereo pairs matrices (color camera preview can be out of FOV, but not the Stereo pairs) -2. Put charuco on a flat surface (bigger board it will be better) -3. When you take a frame is a better practice to freeze board (motion are not ok) - - -#### Some hardware notes: - -##### Peripherals: - -- LCD node - accepts BGR8 image (best if already resized to 160x80px) on /lcd Image topic -- LED node - Subscribes to /led topic, message type is LEDControl (refer to rae_msgs/msg/LEDControl) -- Mic node - Publishes audio_msgs/msg/Audio (from gst_bridge package) on /audio_in, configuration is S32_LE, 48kHz, 2 channel interleaved -- Speakers node - Subscibes to audio_out to same type as Mic node, configuration is S16_LE, 41kHz, 2 channel interleaved - -##### GST-ROS bridge -You can use gst-bridge for testing, for example to play audio on a ros topic: -- `gst-launch-1.0 --gst-plugin-path=install/gst_bridge/lib/gst_bridge/ filesrc location=sample.mp3 ! decodebin ! audioconvert ! rosaudiosink ros-topic="/audio_out"` - -- `gst-launch-1.0 --gst-plugin-path=install/gst_bridge/lib/gst_bridge/ rosaudiosrc ros-topic="audio_out" ! audioconvert ! wavenc ! filesink location=mic1.wav` - -- `gst-launch-1.0 --gst-plugin-path=install/gst_bridge/lib/gst_bridge/ rosimagesrc ros-topic="/rae/right_front/image_raw" ! videoconvert ! videoscale ! video/x-raw,width=160,height=80 ! fbdevsink` - -- `gst-launch-1.0 alsasrc device="hw:0,1" ! audio/x-raw,rate=48000,format=S32LE ! audioconvert ! spectrascope ! videoconvert ! video/x-raw,width=160,height=80 ! fbdevsink` -##### Sensors and sockets -Socket 1 - OV9782 -Socket 0 - IMX214 -Socket 2 - OV9782 -Socket 3 - OV9782 -Socket 4 - OV9782 - -#### Testing motors - -In `rae_hw/test` you can find three scripts that will help you verify that the motors are running correctly. If you want to change arguments, you need to provide all of them. - -1. To find out if encoder is working accurately, execute `ros2 run rae_hw test_encoders` and rotate the wheel by 360 degrees. After rotation, encoder readout should be ~2PI. If not, adjust encoder tick per rev parameter. -Scipt arguments - `[encRatioL encRatioR]`. Full arg version `ros2 run rae_hw test_encoders 756 756` -2. Finding out max speed - `ros2 run rae_hw test_max_speed`. Script arguments `[duration encRatioL encRatioR]`. Full arg version `ros2 run rae_hw test_max_speed 1.0 756 756` -3. Motor verification - `ros2 run rae_hw test_motors`. Script arguments `[duration speedL speedR encRatioL encRatioR maxVelL maxVelR]`. Full arg version `ros2 run rae_hw test_motors 5.0 16.0 16.0 756 756 32 32` - -##### Motors: -Motor configuration parameters are provided in `rae_description/urdf/rae_ros2_control.urdf.xacro` file. -Pin numbers shouldn't change between devices, but if that's the case you can edit that file to set new ones. -- PWM pins (speed control): -``` -2 -1 -``` -- Phase pins (direction control) -``` -41 -45 -``` -- Encoder pins - each motor has A and B pins for encoders. -``` -42 -43 -46 -47 -``` -- How many encoder tics are there per revolution - this might vary from setup to setup. To verify that, run the controller and rotate a wheel manually. You can see current positions/velocities by listening on `/joint_states` topic - `ros2 topic echo /joint_states`. -``` -756 -756 -``` -- Max motor speed in rads/s -``` -32 -32 -``` - -- Both wheels have parameters for PID control set in that file, those values could need some tuning: - ``` - 1 - 0.2 - 0.1 - 0.0005 - ``` - -Parameters for differential driver controller are present in `rae_hw/config/controller.yaml`. `wheel_separation` and `wheel_radius` parameters might also need tuning depending on the setup. - -Implementation of motor control is found in rae_hw package. You can set the motor to be ready to recieve twist commands on /cmd_vel topic by running: - - `ros2 launch rae_hw control.launch.py` - -You can then control the robot via keyboard teleopt from your pc via (assuming you are connected to same network robot is in): -1. sudo apt-get install ros-humble-teleop-twist-keyboard -2. ros2 run teleop_twist_keyboard teleop_twist_keyboard - -If keyboard is too limitng for your tests you could also use ros-humble-teleop-twist-joy and connect a joystick. - -#### LED node - -Under rae_msgs package you can find custom messages that let you control LED lights around the robot. Under those messages there are 3 control types: - -1. Control all (set control_type to 0) gives all LEDs the same color -2. Control single (control_type to 1) lets you control just a single LED light by setting single_led_n variable -3. Custom control (control_type to 2) where you send a list that defines every value of LED lights at once. - -Useful example of how to work with LEDs can be found in rae_bringup/scripts/led_test.py where this for loop is populating LED values (with custom control) for each individual LED: - -``` - for i in range(40): - led_msg.single_led_n = 0 - led_msg.control_type = 2 - if i < 8: - color = "white" - led_msg.data[i]=(colors[color]) - if i >9 and i < 14 and angular_speed > 0.0 and blinking==True: - color = "yellow" - led_msg.data[i]=(colors[color]) - if i > 20 and i < 29 and linear_speed < 0.0: - color = "red" - led_msg.data[i]=(colors[color]) - if i> 34 and i < 39 and angular_speed < 0.0 and blinking==True: - color = "yellow" - led_msg.data[i]=(colors[color]) -``` -We can then send that message to a topic that LED node listens to - by default that is /leds . Easiest way to run LED node (and all other peripheral node) is to run one of the following launch files: - -``` -ros2 launch rae_hw peripherals.launch.py -ros2 launch rae_hw control.launch.py -ros2 launch rae_bringup robot.launch.py -``` - -Where first file is running only peripherals, 2nd is running peripherals and motors, while 3rd is running both of those along with cameras. It could be useful to create your own launch file that mimics peripherals launch file, so you can run your own examples along with nodes. - -### LCD node - -LCD node is listening to messages on /lcd topic and showing it on the screen in the. Useful demo for this node is in rae_bringup/scripts/battery_status.py where we subscribe to battery status topic and based on that create image of battery status on LCD screen. This node expects ROS (sensor_msg/Image) image, so you will generally have to transform opencv imaget to ROS image: - -`img_msg = self.bridge.cv2_to_imgmsg(img_cv, encoding="bgr8")` - -### Microcphone and speakers - -Microphone node expects audio messages and example of how to use that data (along with some other peripherals) can be found in rae_bringup/scripts/audio_spectrum.py . For fair amount of use caes, you will need to decode incoming data as shown in example below. - -``` - if msg.encoding == "S32LE": - audio_data = np.frombuffer(msg.data, dtype=np.int32) - elif msg.encoding == "S16LE": - audio_data = np.frombuffer(msg.data, dtype=np.int16) - if msg.layout == Audio.LAYOUT_INTERLEAVED: - # Deinterleave channels - audio_data = audio_data.reshape((msg.frames, msg.channels)) -``` - - - -Speakers operate similarly, in that they output audio messages. In bringup package in scripts folder sound_test.py offers a decent example of how you can create audio messages. We will shortly create more demos for speakers and microphone. - +- [Getting started](https://docs-beta.luxonis.com/hardware/rae/get-started/) +- [RAE SDK](https://docs-beta.luxonis.com/develop/rae/rae-sdk/) +- [RAE ROS](https://docs-beta.luxonis.com/develop/rae/rae-ros/) +- [Hardware](https://docs-beta.luxonis.com/hardware/products/Rae) +- [Updating firmware](https://docs-beta.luxonis.com/hardware/rae/firmware/) \ No newline at end of file diff --git a/rae_hw/CMakeLists.txt b/rae_hw/CMakeLists.txt index c779fbd..ba4fd96 100644 --- a/rae_hw/CMakeLists.txt +++ b/rae_hw/CMakeLists.txt @@ -33,9 +33,6 @@ ament_auto_add_library( SHARED src/rae_hw.cpp src/rae_motors.cpp - src/peripherals/battery.cpp - src/peripherals/lcd.cpp - src/peripherals/led.cpp ) ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES} ALSA std_srvs) @@ -50,11 +47,6 @@ target_link_libraries( # prevent pluginlib from using boost target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::BatteryNode") -rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::LCDNode") -rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::LEDNode") -rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::SpeakersNode") - pluginlib_export_plugin_description_file( hardware_interface rae_hw.xml) @@ -66,14 +58,29 @@ install( LIBRARY DESTINATION lib ) -add_executable(mic_node src/mic_node.cpp) +add_executable(mic_node src/peripherals/mic.cpp) ament_target_dependencies(mic_node rclcpp audio_msgs) target_link_libraries(mic_node ${PROJECT_NAME}) -add_executable(speakers_node src/speakers_node.cpp) +add_executable(speakers_node src/peripherals/speakers.cpp) ament_target_dependencies(speakers_node rclcpp audio_msgs) target_link_libraries(speakers_node ${PROJECT_NAME}) +add_executable(led_node src/peripherals/led.cpp) +ament_target_dependencies(led_node rclcpp rae_msgs) +target_link_libraries(led_node ${PROJECT_NAME}) + +add_executable(lcd_node src/peripherals/lcd.cpp) +ament_target_dependencies(lcd_node rclcpp sensor_msgs) +target_link_libraries(lcd_node ${PROJECT_NAME}) + +add_executable(battery_node src/peripherals/battery.cpp) +ament_target_dependencies(battery_node rclcpp sensor_msgs) +target_link_libraries(battery_node ${PROJECT_NAME}) + + + + add_executable(test_motors test/test_motors.cpp src/rae_motors.cpp) target_link_libraries(test_motors ${GPIOD_LIBRARY}) @@ -90,7 +97,7 @@ ament_target_dependencies(test_speed rclcpp geometry_msgs) install(TARGETS - test_motors test_encoders test_max_speed test_speed mic_node speakers_node + test_motors test_encoders test_max_speed test_speed mic_node speakers_node led_node lcd_node battery_node DESTINATION lib/${PROJECT_NAME}) @@ -121,7 +128,7 @@ if(BUILD_TESTING) ros2_control_test_assets ) endif() -install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch config assets DESTINATION share/${PROJECT_NAME}) ament_export_include_directories( include ) @@ -147,6 +154,7 @@ install( scripts/mock_mic.py scripts/mock_speakers.py scripts/mock_wheels.py + scripts/lifecycle_manager.py DESTINATION lib/${PROJECT_NAME} ) ament_package() diff --git a/assets/rae-logo-white.jpg b/rae_hw/assets/rae-logo-white.jpg similarity index 100% rename from assets/rae-logo-white.jpg rename to rae_hw/assets/rae-logo-white.jpg diff --git a/rae_hw/assets/shutdown.mp3 b/rae_hw/assets/shutdown.mp3 new file mode 100644 index 0000000..c19d147 Binary files /dev/null and b/rae_hw/assets/shutdown.mp3 differ diff --git a/rae_hw/assets/startup.mp3 b/rae_hw/assets/startup.mp3 new file mode 100644 index 0000000..ce85ffe Binary files /dev/null and b/rae_hw/assets/startup.mp3 differ diff --git a/rae_hw/include/rae_hw/peripherals/battery.hpp b/rae_hw/include/rae_hw/peripherals/battery.hpp index 223e14c..281d5f0 100644 --- a/rae_hw/include/rae_hw/peripherals/battery.hpp +++ b/rae_hw/include/rae_hw/peripherals/battery.hpp @@ -5,14 +5,21 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "sensor_msgs/msg/battery_state.hpp" namespace rae_hw { -class BatteryNode : public rclcpp::Node { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +class BatteryNode : public rclcpp_lifecycle::LifecycleNode { public: BatteryNode(const rclcpp::NodeOptions& options); ~BatteryNode(); + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state); + private: std::string readVarFromFile(const std::string& varName); void logStatus(const sensor_msgs::msg::BatteryState& message); diff --git a/rae_hw/include/rae_hw/peripherals/lcd.hpp b/rae_hw/include/rae_hw/peripherals/lcd.hpp index d197d08..6a8f454 100644 --- a/rae_hw/include/rae_hw/peripherals/lcd.hpp +++ b/rae_hw/include/rae_hw/peripherals/lcd.hpp @@ -8,16 +8,24 @@ #include "cv_bridge/cv_bridge.h" #include "opencv2/opencv.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "sensor_msgs/msg/image.hpp" namespace rae_hw { -class LCDNode : public rclcpp::Node { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +class LCDNode : public rclcpp_lifecycle::LifecycleNode { public: LCDNode(const rclcpp::NodeOptions& options); ~LCDNode(); + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state); + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); void display_image(const cv::Mat& img); + void cleanup(); private: rclcpp::Subscription::SharedPtr subscription_; diff --git a/rae_hw/include/rae_hw/peripherals/led.hpp b/rae_hw/include/rae_hw/peripherals/led.hpp index 1747c6b..d07b9b4 100644 --- a/rae_hw/include/rae_hw/peripherals/led.hpp +++ b/rae_hw/include/rae_hw/peripherals/led.hpp @@ -21,6 +21,7 @@ #include "rae_hw/peripherals/spidev.h" #include "rae_msgs/msg/led_control.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) #define WS2812B_NUM_LEDS 39 @@ -28,11 +29,19 @@ #define WS2812B_BUFFER_SIZE (WS2812B_NUM_LEDS * 24 + WS2812B_RESET_PULSE) namespace rae_hw { -class LEDNode : public rclcpp::Node { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +class LEDNode : public rclcpp_lifecycle::LifecycleNode { public: LEDNode(const rclcpp::NodeOptions& options); ~LEDNode(); + void cleanup(); + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state); + private: std::map logicalToPhysicalMapping; void transmitSPI(); diff --git a/rae_hw/include/rae_hw/peripherals/mic.hpp b/rae_hw/include/rae_hw/peripherals/mic.hpp index 33f3f91..8829d5c 100644 --- a/rae_hw/include/rae_hw/peripherals/mic.hpp +++ b/rae_hw/include/rae_hw/peripherals/mic.hpp @@ -10,15 +10,23 @@ #include "rae_msgs/action/recording.hpp" #include "rae_msgs/msg/rae_audio.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_srvs/srv/trigger.hpp" namespace rae_hw { - -class MicNode : public rclcpp::Node { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +class MicNode : public rclcpp_lifecycle::LifecycleNode { public: MicNode(const rclcpp::NodeOptions& options); ~MicNode(); + void cleanup(); + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state); + private: void configure_microphone(); void timer_callback(); diff --git a/rae_hw/include/rae_hw/peripherals/speakers.hpp b/rae_hw/include/rae_hw/peripherals/speakers.hpp index 78991fa..9f9da82 100644 --- a/rae_hw/include/rae_hw/peripherals/speakers.hpp +++ b/rae_hw/include/rae_hw/peripherals/speakers.hpp @@ -8,21 +8,27 @@ #include "audio_msgs/msg/audio.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/int32.hpp" namespace rae_hw { - -class SpeakersNode : public rclcpp::Node { +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +class SpeakersNode : public rclcpp_lifecycle::LifecycleNode { public: SpeakersNode(const rclcpp::NodeOptions& options); ~SpeakersNode(); + void cleanup(); + + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state); + CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state); private: void play_mp3(const char*); rclcpp::Service::SharedPtr play_audio_service_; - void play_audio_service_callback(const std::shared_ptr request_header, - const std::shared_ptr request, + void play_audio_service_callback(const std::shared_ptr request, const std::shared_ptr response); snd_pcm_t* alsaHandle; diff --git a/rae_hw/launch/control.launch.py b/rae_hw/launch/control.launch.py index ae4b2c4..0217def 100644 --- a/rae_hw/launch/control.launch.py +++ b/rae_hw/launch/control.launch.py @@ -4,10 +4,11 @@ from launch import LaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.actions import IncludeLaunchDescription, OpaqueFunction, DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription, OpaqueFunction, DeclareLaunchArgument, RegisterEventHandler from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition -from launch_ros.actions import Node +from launch_ros.event_handlers import OnStateTransition +from launch_ros.actions import Node, LifecycleNode import xacro @@ -16,80 +17,135 @@ def launch_setup(context, *args, **kwargs): 'rae_description'), 'urdf', 'rae.urdf.xacro') rae_description_config = xacro.process_file(rae_description_path) robot_description = {'robot_description': rae_description_config.toxml()} - controller_params = LaunchConfiguration('controller_params_file').perform(context) + controller_params = LaunchConfiguration( + 'controller_params_file').perform(context) run_container = LaunchConfiguration('run_container', default='true') enable_localization = LaunchConfiguration( 'enable_localization', default='true') - return [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('rae_description'), 'launch', 'rsp.launch.py')), - launch_arguments={'sim': 'false', - 'namespace': LaunchConfiguration('namespace'), - }.items() - ), - Node( - condition=IfCondition(enable_localization), - package='robot_localization', - executable='ekf_node', - name='ekf_filter_node', - namespace=LaunchConfiguration('namespace'), - output='screen', - parameters=[os.path.join(get_package_share_directory( + + robot_state_pub = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('rae_description'), 'launch', 'rsp.launch.py')), + launch_arguments={'sim': 'false', + 'namespace': LaunchConfiguration('namespace'), + }.items() + ) + + ekf_node = Node( + condition=IfCondition(enable_localization), + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + namespace=LaunchConfiguration('namespace'), + output='screen', + parameters=[os.path.join(get_package_share_directory( 'rae_hw'), 'config', 'ekf.yaml')], - ), - Node( - package='imu_complementary_filter', - executable='complementary_filter_node', - name='complementary_filter_gain_node', - namespace=LaunchConfiguration('namespace'), - output='screen', - parameters=[ - {'do_bias_estimation': True}, - {'do_adaptive_gain': True}, - {'orientation_stddev': 0.001}, - {'use_mag': False}, - {'gain_acc': 0.04}, - {'gain_mag': 0.01}, - ], - remappings=[ - ('imu/data_raw', '/rae/imu/data'), - ] - ), - Node( - package='controller_manager', - executable='ros2_control_node', - namespace=LaunchConfiguration('namespace'), - parameters=[robot_description, controller_params], - remappings=[('/diff_controller/cmd_vel_unstamped', 'cmd_vel')], - output={ + ) + + imu_comp_filt = Node( + package='imu_complementary_filter', + executable='complementary_filter_node', + name='complementary_filter_gain_node', + namespace=LaunchConfiguration('namespace'), + output='screen', + parameters=[ + {'do_bias_estimation': True}, + {'do_adaptive_gain': True}, + {'orientation_stddev': 0.001}, + {'use_mag': False}, + {'gain_acc': 0.04}, + {'gain_mag': 0.01}, + ], + remappings=[ + ('imu/data_raw', '/rae/imu/data'), + ] + ) + + controller_manager = Node( + package='controller_manager', + executable='ros2_control_node', + namespace=LaunchConfiguration('namespace'), + parameters=[robot_description, controller_params], + remappings=[('/diff_controller/cmd_vel_unstamped', 'cmd_vel')], + output={ 'stdout': 'screen', 'stderr': 'screen', - }, - ), - - Node( - package='controller_manager', - namespace=LaunchConfiguration('namespace'), - executable='spawner', - arguments=['diff_controller', '-c', '/controller_manager'], - ), - - Node( - package='controller_manager', - executable='spawner', - namespace=LaunchConfiguration('namespace'), - arguments=['joint_state_broadcaster', - '--controller-manager', '/controller_manager'], - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('rae_hw'), 'launch', 'peripherals.launch.py')), - launch_arguments={'run_container': run_container, - 'namespace': LaunchConfiguration('namespace'), - }.items() - ), + }, + ) + + diff_controller = Node( + package='controller_manager', + namespace=LaunchConfiguration('namespace'), + executable='spawner', + arguments=['diff_controller', '-c', '/controller_manager'], + ) + + joint_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + namespace=LaunchConfiguration('namespace'), + arguments=['joint_state_broadcaster', + '--controller-manager', '/controller_manager'], + ) + lifecycle_manager = Node( + package='rae_hw', + executable='lifecycle_manager.py', + name='lifecycle_manager', + namespace=LaunchConfiguration('namespace') + ) + lcd = LifecycleNode( + name='lcd_node', + namespace=LaunchConfiguration('namespace'), + package='rae_hw', + executable='lcd_node', + ) + led = LifecycleNode( + name='led_node', + namespace=LaunchConfiguration('namespace'), + package='rae_hw', + executable='led_node', + ) + speakers = LifecycleNode( + name='speakers_node', + namespace=LaunchConfiguration('namespace'), + package='rae_hw', + executable='speakers_node', + ) + + mic = LifecycleNode( + name='mic_node', + namespace=LaunchConfiguration('namespace'), + package='rae_hw', + executable='mic_node', + ) + + battery = LifecycleNode( + name='battery_node', + namespace=LaunchConfiguration('namespace'), + package='rae_hw', + executable='battery_node', + ) + + return [ + lifecycle_manager, + led, + lcd, + RegisterEventHandler(OnStateTransition( + target_lifecycle_node=led, + goal_state='active', + entities=[battery, + mic, + speakers, + robot_state_pub, + ekf_node, + imu_comp_filt, + controller_manager, + diff_controller, + joint_state_broadcaster, + ] + )) + ] @@ -101,7 +157,7 @@ def generate_launch_description(): DeclareLaunchArgument('enable_battery_status', default_value='true'), DeclareLaunchArgument('enable_localization', default_value='true'), DeclareLaunchArgument('controller_params_file', default_value=os.path.join(get_package_share_directory( - 'rae_hw'), 'config', 'controller.yaml')), + 'rae_hw'), 'config', 'controller.yaml')), ] return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/rae_hw/launch/control_mock.launch.py b/rae_hw/launch/control_mock.launch.py index 04e3f2b..722f145 100644 --- a/rae_hw/launch/control_mock.launch.py +++ b/rae_hw/launch/control_mock.launch.py @@ -7,7 +7,7 @@ from launch.actions import IncludeLaunchDescription, OpaqueFunction, DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition -from launch_ros.actions import Node +from launch_ros.actions import Node, LifecycleNode import xacro @@ -23,39 +23,46 @@ def launch_setup(context, *args, **kwargs): }.items() ), Node( + package='rae_hw', + executable='lifecycle_manager.py', + name='lifecycle_manager', + namespace=LaunchConfiguration('namespace'), + parameters=[{'mock': True}] + ), + LifecycleNode( package='rae_hw', executable='mock_wheels.py', - name='mock_wheels', + name='diff_controller', namespace=LaunchConfiguration('namespace') ), - Node( + LifecycleNode( package='rae_hw', executable='mock_leds.py', - name='mock_leds', + name='led_node', namespace=LaunchConfiguration('namespace') ), - Node( + LifecycleNode( package='rae_hw', executable='mock_battery.py', - name='mock_battery', + name='battery_node', namespace=LaunchConfiguration('namespace') ), - Node( + LifecycleNode( package='rae_hw', executable='mock_lcd.py', - name='mock_lcd', + name='lcd_node', namespace=LaunchConfiguration('namespace') ), - Node( + LifecycleNode( package='rae_hw', executable='mock_speakers.py', - name='mock_speakers', + name='speakers_node', namespace=LaunchConfiguration('namespace') ), - Node( + LifecycleNode( package='rae_hw', executable='mock_mic.py', - name='mock_mic', + name='mic_node', namespace=LaunchConfiguration('namespace') ) ] diff --git a/rae_hw/launch/peripherals.launch.py b/rae_hw/launch/peripherals.launch.py deleted file mode 100644 index e41e5a4..0000000 --- a/rae_hw/launch/peripherals.launch.py +++ /dev/null @@ -1,70 +0,0 @@ - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch_ros.actions import Node, LoadComposableNodes, ComposableNodeContainer -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.descriptions import ComposableNode - - -def launch_setup(context, *args, **kwargs): - name = LaunchConfiguration('name').perform(context) - run_container = LaunchConfiguration('run_container', default=True) - return [ - ComposableNodeContainer( - condition=IfCondition(run_container), - name=name+'_container', - namespace=LaunchConfiguration('namespace'), - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[], - output='both', - ), - LoadComposableNodes( - target_container=name+'_container', - composable_node_descriptions=[ - ComposableNode( - name='battery_node', - namespace=LaunchConfiguration('namespace'), - package='rae_hw', - plugin='rae_hw::BatteryNode', - ), - ComposableNode( - name='lcd_node', - namespace=LaunchConfiguration('namespace'), - package='rae_hw', - plugin='rae_hw::LCDNode', - parameters=[{ - 'default_logo_path': LaunchConfiguration('default_logo_path') - }], - ), - ComposableNode( - name='led_node', - namespace=LaunchConfiguration('namespace'), - package='rae_hw', - plugin='rae_hw::LEDNode', - ) - - ]), - Node( - package='rae_hw', - executable='speakers_node' - ), - Node( - package='rae_hw', - executable='mic_node' - ), - ] - - -def generate_launch_description(): - declared_arguments = [ - DeclareLaunchArgument('name', default_value='rae'), - DeclareLaunchArgument('namespace', default_value=''), - DeclareLaunchArgument('run_container', default_value='true'), - DeclareLaunchArgument('default_logo_path', default_value='/usr/share/rae-logo-white.jpg'), - ] - - return LaunchDescription( - declared_arguments + [OpaqueFunction(function=launch_setup)] - ) diff --git a/rae_hw/scripts/lifecycle_manager.py b/rae_hw/scripts/lifecycle_manager.py new file mode 100755 index 0000000..9ece79d --- /dev/null +++ b/rae_hw/scripts/lifecycle_manager.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 + +import os +import rclpy +import cv2 + +from rclpy.node import Node + +from lifecycle_msgs.msg import Transition +from lifecycle_msgs.srv import ChangeState +from rae_msgs.msg import LEDControl, ColorPeriod +from sensor_msgs.msg import Image +from rae_msgs.srv import PlayAudio +from std_msgs.msg import String +from controller_manager_msgs.srv import ListControllers +from cv_bridge import CvBridge +from ament_index_python import get_package_share_directory + + +class LifecycleManager(Node): + def __init__(self): + super().__init__('rae_hw_lifecycle_manager') + self.get_logger().info('Initializing Lifecycle Manager') + self._mock = self.declare_parameter('mock', False).value + self._silent_startup = self.declare_parameter( + 'silent_startup', False).value + self._startup_sound_path = self.declare_parameter('startup_sound_path', os.path.join( + get_package_share_directory('rae_hw'), 'assets', 'startup.mp3')).value + self._node_names = ['mic_node', 'battery_node', 'speakers_node'] + + # for each node, create a service client to change state + self._change_state_clients = {} + for node_name in self._node_names: + self._change_state_clients[node_name] = self.create_client( + ChangeState, node_name + '/change_state') + self._change_state_clients['led_node'] = self.create_client( + ChangeState, 'led_node/change_state') + self._change_state_clients['lcd_node'] = self.create_client( + ChangeState, 'lcd_node/change_state') + self._controllers_client = self.create_client( + ListControllers, 'controller_manager/list_controllers') + self._led_pub = self.create_publisher(LEDControl, 'leds', 10) + self._lcd_pub = self.create_publisher(Image, 'lcd', 10) + self._progress = 0 + self._bridge = CvBridge() + self._ready_publisher = self.create_publisher(String, 'ready', 10) + self._audio_client = self.create_client(PlayAudio, 'play_audio') + self.get_logger().info('Lifecycle Manager initialized') + + def startup(self): + self.get_logger().info('Starting up...') + self.startup_visual_nodes() + self.startup_rest() + + self.check_diff_controller() + + self.finish_indicator() + + self._ready_publisher.publish(String(data='ready')) + self.get_logger().info('Startup complete') + + def startup_visual_nodes(self): + self.get_logger().info('Startup LED node') + self.configure_and_activate('led_node') + self.configure_and_activate('lcd_node') + + def startup_rest(self): + for node_name in self._node_names: + self.get_logger().info(f'Startup {node_name}') + self.configure_and_activate(node_name) + self.update_progress_indicators() + + def check_diff_controller(self): + if self._mock: + return + self.get_logger().info('Checking diff controller...') + diff_controller_running = False + while not diff_controller_running: + self._controllers_client.wait_for_service() + req = ListControllers.Request() + future = self._controllers_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + for controller in future.result().controller: + if controller.name == 'diff_controller': + if controller.state == 'active': + self.get_logger().info('Diff controller running') + diff_controller_running = True + break + else: + self.get_logger().error('Failed to get controller list') + + def finish_indicator(self): + if self._silent_startup: + return + img = cv2.imread(os.path.join(get_package_share_directory( + 'rae_hw'), 'assets', 'rae-logo-white.jpg')) + + # add text to top of the image (image size is 160x80) + cv2.putText(img, 'READY', (54, 16), + cv2.FONT_HERSHEY_DUPLEX, 0.5, (150, 240, 110), 1, cv2.LINE_AA) + + img_msg = self._bridge.cv2_to_imgmsg(img, 'bgr8') + + self._lcd_pub.publish(img_msg) + + color = ColorPeriod() + color.color.a = 1.0 + color.color.g = 1.0 + color.frequency = 1.0 + + msg = LEDControl() + msg.data = [color] + msg.control_type = LEDControl.CTRL_TYPE_ALL + msg.animation_size = 1 + msg.animation_quantity = 40 + self._led_pub.publish(msg) + + # send startup sound + req = PlayAudio.Request() + req.mp3_file = self._startup_sound_path + self._audio_client.wait_for_service() + future = self._audio_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + + def update_progress_indicators(self): + if self._silent_startup: + return + self.get_logger().info('Updating indicators...') + color = ColorPeriod() + color.color.a = 1.0 + color.color.b = 1.0 + color.frequency = 1.0 + + msg = LEDControl() + msg.data = [color] + msg.control_type = LEDControl.CTRL_TYPE_SPINNER + msg.animation_size = 1 + msg.animation_quantity = self._progress + 1 + self._led_pub.publish(msg) + + img = cv2.imread(os.path.join(get_package_share_directory( + 'rae_hw'), 'assets', 'rae-logo-white.jpg')) + + # add text to top of the image (image size is 160x80) + cv2.putText(img, 'LOADING', (45, 16), + cv2.FONT_HERSHEY_DUPLEX, 0.5, (150, 240, 110), 1, cv2.LINE_AA) + + # add loading bar to the bottom of the image + # add loading bar border + cv2.rectangle(img, (18, 64), (142, 76), (255, 255, 255), 1) + + cv2.rectangle( + img, (20, 66), (20 + self._progress*40, 74), (150, 240, 110), -1) + + img_msg = self._bridge.cv2_to_imgmsg(img, 'bgr8') + self._lcd_pub.publish(img_msg) + self._progress += 1 + + def send_transition(self, node_name, transition): + self.get_logger().info( + f'Sending transition {transition} to {node_name}...') + self._change_state_clients[node_name].wait_for_service() + transition_msg = Transition() + transition_msg.id = transition + + req = ChangeState.Request() + req.transition = transition_msg + future = self._change_state_clients[node_name].call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + self.get_logger().info( + f'Successfully transitioned {node_name} to {transition}') + else: + self.get_logger().error( + f'Failed to transition {node_name} to {transition}') + + def configure_and_activate(self, node_name): + self.send_transition(node_name, Transition.TRANSITION_CONFIGURE) + self.send_transition(node_name, Transition.TRANSITION_ACTIVATE) + + +def main(args=None): + rclpy.init(args=args) + node = LifecycleManager() + node.startup() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/rae_hw/scripts/mock_battery.py b/rae_hw/scripts/mock_battery.py index 9474da2..7df13eb 100755 --- a/rae_hw/scripts/mock_battery.py +++ b/rae_hw/scripts/mock_battery.py @@ -1,22 +1,41 @@ #!/usr/bin/env python3 +from time import sleep + import rclpy -from rclpy.node import Node +from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn +from rclpy.lifecycle import TransitionCallbackReturn, Node from sensor_msgs.msg import BatteryState class MockBattery(Node): def __init__(self): - super().__init__('mock_battery') - self._battery_pub = self.create_publisher(BatteryState, 'battery', 10) - self.timer = self.create_timer(1, self.timer_callback) + super().__init__('battery_node') def timer_callback(self): msg = BatteryState() self._battery_pub.publish(msg) + def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Configuring') + self._battery_pub = self.create_publisher(BatteryState, 'battery', 10) + self.timer = self.create_timer(1, self.timer_callback) + sleep(0.5) + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Activating') + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Deactivating') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Shutting down') + return TransitionCallbackReturn.SUCCESS + if __name__ == '__main__': rclpy.init() mock_battery = MockBattery() rclpy.spin(mock_battery) - mock_battery.destroy_node() rclpy.shutdown() \ No newline at end of file diff --git a/rae_hw/scripts/mock_lcd.py b/rae_hw/scripts/mock_lcd.py index fc08a19..fe2a1fa 100755 --- a/rae_hw/scripts/mock_lcd.py +++ b/rae_hw/scripts/mock_lcd.py @@ -1,19 +1,36 @@ #!/usr/bin/env python3 import rclpy -from rclpy.node import Node +from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn +from rclpy.lifecycle import TransitionCallbackReturn, Node from sensor_msgs.msg import Image class MockLCD(Node): def __init__(self): - super().__init__('mock_display') - self._image_sub = self.create_subscription(Image, 'lcd', self.image_callback, 10) + super().__init__('lcd_node') + def image_callback(self, msg: Image): self.get_logger().info(f'I heard: {msg.width}, {msg.height}') + def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Configuring') + self._image_sub = self.create_subscription(Image, 'lcd', self.image_callback, 10) + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Activating') + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Deactivating') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Shutting down') + return TransitionCallbackReturn.SUCCESS + if __name__ == '__main__': rclpy.init() mock_lcd = MockLCD() rclpy.spin(mock_lcd) - mock_lcd.destroy_node() rclpy.shutdown() \ No newline at end of file diff --git a/rae_hw/scripts/mock_leds.py b/rae_hw/scripts/mock_leds.py index b903e82..f4bcbec 100755 --- a/rae_hw/scripts/mock_leds.py +++ b/rae_hw/scripts/mock_leds.py @@ -1,19 +1,40 @@ #!/usr/bin/env python3 import rclpy -from rclpy.node import Node +from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn +from rclpy.lifecycle import TransitionCallbackReturn, Node from rae_msgs.msg import LEDControl + class MockLeds(Node): def __init__(self): - super().__init__('mock_leds') - self._led_sub = self.create_subscription(LEDControl, 'leds', self.led_callback, 10) + super().__init__('led_node') + + def led_callback(self, msg: LEDControl): - self.get_logger().info('I heard: "%s"' % msg.data) + self.get_logger().info('I heard: "%s"' % msg) + + def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Configuring') + self._led_sub = self.create_subscription( + LEDControl, 'leds', self.led_callback, 10) + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Activating') + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Deactivating') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Shutting down') + return TransitionCallbackReturn.SUCCESS + if __name__ == '__main__': rclpy.init() mock_leds = MockLeds() rclpy.spin(mock_leds) - mock_leds.destroy_node() - rclpy.shutdown() \ No newline at end of file + rclpy.shutdown() diff --git a/rae_hw/scripts/mock_mic.py b/rae_hw/scripts/mock_mic.py index a934b02..55b60d3 100755 --- a/rae_hw/scripts/mock_mic.py +++ b/rae_hw/scripts/mock_mic.py @@ -1,22 +1,40 @@ #!/usr/bin/env python3 +from time import sleep import rclpy -from rclpy.node import Node +from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn +from rclpy.lifecycle import TransitionCallbackReturn, Node from audio_msgs.msg import Audio class MockMic(Node): def __init__(self): - super().__init__('mock_mic') - self._audio_pub = self.create_publisher(Audio, 'audio_in', 10) - self.timer = self.create_timer(1, self.timer_callback) + super().__init__('mic_node') def timer_callback(self): msg = Audio() self._audio_pub.publish(msg) + def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Configuring') + self._audio_pub = self.create_publisher(Audio, 'audio_in', 10) + self.timer = self.create_timer(1, self.timer_callback) + sleep(0.5) + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Activating') + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Deactivating') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Shutting down') + return TransitionCallbackReturn.SUCCESS + if __name__ == '__main__': rclpy.init() mock_mic = MockMic() rclpy.spin(mock_mic) - mock_mic.destroy_node() rclpy.shutdown() \ No newline at end of file diff --git a/rae_hw/scripts/mock_speakers.py b/rae_hw/scripts/mock_speakers.py index 3ff7b15..a583d9a 100755 --- a/rae_hw/scripts/mock_speakers.py +++ b/rae_hw/scripts/mock_speakers.py @@ -1,18 +1,15 @@ #!/usr/bin/env python3 +from time import sleep import rclpy -from rclpy.node import Node +from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn +from rclpy.lifecycle import TransitionCallbackReturn, Node from audio_msgs.msg import Audio from rae_msgs.srv import PlayAudio - class MockSpeakers(Node): def __init__(self): - super().__init__('mock_speakers') - self._audio_sub = self.create_subscription( - Audio, 'audio_out', self.audio_callback, 10) - self._play_service = self.create_service( - PlayAudio, 'play_audio', self.play_callback) + super().__init__('speakers_node') def audio_callback(self, msg: Audio): self.get_logger().info('I heard: "%s"' % msg.data) @@ -20,11 +17,30 @@ def audio_callback(self, msg: Audio): def play_callback(self, request: PlayAudio.Request, response): self.get_logger().info('I heard: "%s"' % request.mp3_file) return response + + def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Configuring') + self._audio_sub = self.create_subscription( + Audio, 'audio_out', self.audio_callback, 10) + self._play_service = self.create_service( + PlayAudio, 'play_audio', self.play_callback) + sleep(0.5) + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Activating') + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Deactivating') + return TransitionCallbackReturn.SUCCESS + def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Shutting down') + return TransitionCallbackReturn.SUCCESS if __name__ == '__main__': rclpy.init() mock_speakers = MockSpeakers() rclpy.spin(mock_speakers) - mock_speakers.destroy_node() rclpy.shutdown() diff --git a/rae_hw/scripts/mock_wheels.py b/rae_hw/scripts/mock_wheels.py index 18c44d7..ebf1418 100755 --- a/rae_hw/scripts/mock_wheels.py +++ b/rae_hw/scripts/mock_wheels.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import rclpy -from rclpy.node import Node +from rclpy.lifecycle.node import LifecycleState, TransitionCallbackReturn +from rclpy.lifecycle import TransitionCallbackReturn, Node from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry @@ -8,11 +9,8 @@ class MockWheels(Node): def __init__(self): - super().__init__('mock_wheels') - self._odom_pub = self.create_publisher(Odometry, 'diff_controller/odom', 10) - self._tf_broadcaster = TransformBroadcaster(self) - self._cmd_vel_sub = self.create_subscription(Twist, 'cmd_vel', self.cmd_vel_callback, 10) - self.timer = self.create_timer(100, self.timer_callback) + super().__init__('diff_controller') + def cmd_vel_callback(self, msg: Twist): self.get_logger().info(f'I heard: {msg.angular.x}, {msg.angular.z}') @@ -25,12 +23,31 @@ def timer_callback(self): tf.header.stamp = self.get_clock().now().to_msg() tf.header.frame_id = "odom" tf.child_frame_id = "base_footprint" - self._tf_broadcaster.sendTransform(tf) + # self._tf_broadcaster.sendTransform(tf) self._odom_pub.publish(msg) + def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn: + self._odom_pub = self.create_publisher(Odometry, 'diff_controller/odom', 10) + self._tf_broadcaster = TransformBroadcaster(self) + self._cmd_vel_sub = self.create_subscription(Twist, 'cmd_vel', self.cmd_vel_callback, 10) + self.timer = self.create_timer(100, self.timer_callback) + + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Activating') + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Deactivating') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn: + self.get_logger().info('Shutting down') + return TransitionCallbackReturn.SUCCESS + if __name__ == '__main__': rclpy.init() mock_wheels = MockWheels() rclpy.spin(mock_wheels) - mock_wheels.destroy_node() rclpy.shutdown() \ No newline at end of file diff --git a/rae_hw/src/peripherals/battery.cpp b/rae_hw/src/peripherals/battery.cpp index aff0996..f406543 100644 --- a/rae_hw/src/peripherals/battery.cpp +++ b/rae_hw/src/peripherals/battery.cpp @@ -6,15 +6,34 @@ #include namespace rae_hw { -BatteryNode::BatteryNode(const rclcpp::NodeOptions& options) : rclcpp::Node("battery", options) { +BatteryNode::BatteryNode(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("battery_node", options) {} +BatteryNode::~BatteryNode() = default; + +CallbackReturn BatteryNode::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) { using namespace std::chrono_literals; publisher = this->create_publisher("battery_status", 10); timer = this->create_wall_timer(500ms, std::bind(&BatteryNode::timerCallback, this)); stateChangeTime = this->get_clock()->now(); lastLogTime = this->get_clock()->now(); - RCLCPP_INFO(this->get_logger(), "Battery node running!"); + RCLCPP_INFO(this->get_logger(), "Battery node configured!"); + return CallbackReturn::SUCCESS; } -BatteryNode::~BatteryNode() = default; + +CallbackReturn BatteryNode::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "Battery node activated!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn BatteryNode::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "Battery node deactivated!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn BatteryNode::on_shutdown(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "Battery node shuttind down!"); + return CallbackReturn::SUCCESS; +} + void BatteryNode::timerCallback() { auto message = sensor_msgs::msg::BatteryState(); message.header.stamp = this->get_clock()->now(); @@ -82,5 +101,14 @@ void BatteryNode::logStatus(const sensor_msgs::msg::BatteryState& message) { } } } // namespace rae_hw -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(rae_hw::BatteryNode); \ No newline at end of file +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + rclcpp::executors::StaticSingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + + return 0; +} diff --git a/rae_hw/src/peripherals/lcd.cpp b/rae_hw/src/peripherals/lcd.cpp index d44c128..62bd935 100644 --- a/rae_hw/src/peripherals/lcd.cpp +++ b/rae_hw/src/peripherals/lcd.cpp @@ -5,34 +5,51 @@ #include #include +#include "ament_index_cpp/get_package_share_directory.hpp" #include "cv_bridge/cv_bridge.h" #include "opencv2/opencv.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/image.hpp" namespace rae_hw { -LCDNode::LCDNode(const rclcpp::NodeOptions& options) : Node("lcd_node", options) { - std::string logo_key = "default_logo_path"; - declare_parameter(logo_key); - default_logo_path = get_parameter(logo_key).as_string(); +LCDNode::LCDNode(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("lcd_node", options) {} + +LCDNode::~LCDNode() { + cleanup(); +} + +void LCDNode::cleanup() { + // Load default image + cv::Mat default_img = cv::imread(default_logo_path); + if(!default_img.empty()) { + display_image(default_img); + } + + munmap(fbp, screensize); + close(fbfd); +} + +CallbackReturn LCDNode::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) { + std::string logo_path = ament_index_cpp::get_package_share_directory("rae_hw") + "/assets/rae-logo-white.jpg"; + default_logo_path = declare_parameter("logo_path", logo_path); // Open the framebuffer device fbfd = open("/dev/fb0", O_RDWR); if(fbfd == -1) { RCLCPP_ERROR(this->get_logger(), "Error: cannot open framebuffer device"); - return; + return CallbackReturn::FAILURE; } // Get fixed screen information if(ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo) == -1) { RCLCPP_ERROR(this->get_logger(), "Error reading fixed information"); - return; + return CallbackReturn::FAILURE; } // Get variable screen information if(ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo) == -1) { RCLCPP_ERROR(this->get_logger(), "Error reading variable information"); - return; + return CallbackReturn::FAILURE; } // Calculate the size of the screen in bytes @@ -42,22 +59,28 @@ LCDNode::LCDNode(const rclcpp::NodeOptions& options) : Node("lcd_node", options) fbp = (char*)mmap(0, screensize, PROT_READ | PROT_WRITE, MAP_SHARED, fbfd, (off_t)0); if(fbp == MAP_FAILED) { RCLCPP_ERROR(this->get_logger(), "Error: failed to map framebuffer device to memory"); - return; + return CallbackReturn::FAILURE; } subscription_ = this->create_subscription("lcd", 10, std::bind(&LCDNode::image_callback, this, std::placeholders::_1)); - RCLCPP_INFO(this->get_logger(), "LCD node running!"); + RCLCPP_INFO(this->get_logger(), "LCD node configured!"); + return CallbackReturn::SUCCESS; } -LCDNode::~LCDNode() { - // Load default image - cv::Mat default_img = cv::imread(default_logo_path); - if(!default_img.empty()) { - display_image(default_img); - } +CallbackReturn LCDNode::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "LCD node activated!"); + return CallbackReturn::SUCCESS; +} - munmap(fbp, screensize); - close(fbfd); +CallbackReturn LCDNode::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "LCD node deactivated!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn LCDNode::on_shutdown(const rclcpp_lifecycle::State& /*previous_state*/) { + cleanup(); + RCLCPP_INFO(this->get_logger(), "LCD node shuttind down!"); + return CallbackReturn::SUCCESS; } void LCDNode::display_image(const cv::Mat& img) { @@ -82,5 +105,14 @@ void LCDNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { }; } // namespace rae_hw -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(rae_hw::LCDNode); +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + rclcpp::executors::StaticSingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + + return 0; +} diff --git a/rae_hw/src/peripherals/led.cpp b/rae_hw/src/peripherals/led.cpp index 578b0c0..8cf723c 100644 --- a/rae_hw/src/peripherals/led.cpp +++ b/rae_hw/src/peripherals/led.cpp @@ -2,10 +2,23 @@ namespace rae_hw { LEDNode::LEDNode(const rclcpp::NodeOptions& options) - : Node("led_node", options), logicalToPhysicalMapping{{0, 0}, {1, 1}, {2, 2}, {3, 3}, {4, 4}, {5, 5}, {6, 6}, {7, 7}, {8, 8}, {9, 29}, - {10, 30}, {11, 31}, {12, 32}, {13, 33}, {14, 34}, {15, 35}, {16, 36}, {17, 37}, {18, 38}, {19, 19}, - {20, 20}, {21, 21}, {22, 22}, {23, 23}, {24, 24}, {25, 25}, {26, 26}, {27, 27}, {28, 28}, {29, 9}, - {30, 10}, {31, 11}, {32, 12}, {33, 13}, {34, 14}, {35, 15}, {36, 16}, {37, 17}, {38, 18}} { + : rclcpp_lifecycle::LifecycleNode("led_node", options), logicalToPhysicalMapping{{0, 0}, {1, 1}, {2, 2}, {3, 3}, {4, 4}, {5, 5}, {6, 6}, + {7, 7}, {8, 8}, {9, 29}, {10, 30}, {11, 31}, {12, 32}, {13, 33}, + {14, 34}, {15, 35}, {16, 36}, {17, 37}, {18, 38}, {19, 19}, {20, 20}, + {21, 21}, {22, 22}, {23, 23}, {24, 24}, {25, 25}, {26, 26}, {27, 27}, + {28, 28}, {29, 9}, {30, 10}, {31, 11}, {32, 12}, {33, 13}, {34, 14}, + {35, 15}, {36, 16}, {37, 17}, {38, 18}} {} +LEDNode::~LEDNode() { + cleanup(); +} + +void LEDNode::cleanup() { + setAllPixels(0, 0, 0, 0); + transmitSPI(); + close(fd); +} + +CallbackReturn LEDNode::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) { int ret = 0; memset(ws2812b_buffer, 0, WS2812B_BUFFER_SIZE); fd = open("/dev/spidev3.0", O_RDWR); @@ -35,12 +48,26 @@ LEDNode::LEDNode(const rclcpp::NodeOptions& options) transmitSPI(); timer_ = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&LEDNode::timer_callback, this)); - RCLCPP_INFO(this->get_logger(), "LED node running!"); + RCLCPP_INFO(this->get_logger(), "LED node configured!"); + return CallbackReturn::SUCCESS; } -LEDNode::~LEDNode() { - setAllPixels(0, 0, 0, 0); - transmitSPI(); + +CallbackReturn LEDNode::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "LED node activated!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn LEDNode::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "LED node deactivated!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn LEDNode::on_shutdown(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "LED node shuttind down!"); + cleanup(); + return CallbackReturn::SUCCESS; } + void LEDNode::topic_callback(const rae_msgs::msg::LEDControl::SharedPtr msg) { std::lock_guard lock(mutex_); currentData_ = msg; @@ -169,13 +196,22 @@ void LEDNode::fan(uint8_t r, uint8_t g, uint8_t b, float a, bool opening, uint8_ uint8_t blade_length = WS2812B_NUM_LEDS / blades; uint8_t tip = frame % blade_length; setAllPixels(0, 0, 0, 0); - for(uint32_t i = 0; i < blades; i++) { - for(uint32_t j = 0; j < (opening ? tip : blade_length - tip); j++) { + for(uint8_t i = 0; i < blades; i++) { + for(uint8_t j = 0; j < (opening ? tip : blade_length - tip); j++) { setSinglePixel((j + i * blade_length) % WS2812B_NUM_LEDS, r, g, b, a, frequency); }; }; }; } // namespace rae_hw -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(rae_hw::LEDNode); \ No newline at end of file +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(rclcpp::NodeOptions()); + rclcpp::executors::StaticSingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); + rclcpp::shutdown(); + + return 0; +} diff --git a/rae_hw/src/mic_node.cpp b/rae_hw/src/peripherals/mic.cpp similarity index 82% rename from rae_hw/src/mic_node.cpp rename to rae_hw/src/peripherals/mic.cpp index 2019687..dde4aac 100644 --- a/rae_hw/src/mic_node.cpp +++ b/rae_hw/src/peripherals/mic.cpp @@ -1,12 +1,22 @@ +#include "rae_hw/peripherals/mic.hpp" + #include #include "alsa/asoundlib.h" #include "alsa/pcm.h" -#include "rae_hw/peripherals/mic.hpp" #include "rclcpp/rclcpp.hpp" namespace rae_hw { -MicNode::MicNode(const rclcpp::NodeOptions& options) : Node("mic_node", options), handle_(nullptr) { +MicNode::MicNode(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("mic_node", options), handle_(nullptr) {} +MicNode::~MicNode() { + cleanup(); +} + +void MicNode::cleanup() { + snd_pcm_close(handle_); +} + +CallbackReturn MicNode::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) { publisher_ = this->create_publisher("audio_in", 10); configure_microphone(); @@ -14,10 +24,24 @@ MicNode::MicNode(const rclcpp::NodeOptions& options) : Node("mic_node", options) wav_filename_ = "/tmp/recording.wav"; start_service_ = this->create_service("start_recording", std::bind(&MicNode::startRecording, this, std::placeholders::_1, std::placeholders::_2)); - RCLCPP_INFO(this->get_logger(), "Mic node running!"); + RCLCPP_INFO(this->get_logger(), "Mic node configured!"); + return CallbackReturn::SUCCESS; } -MicNode::~MicNode() { - snd_pcm_close(handle_); + +CallbackReturn MicNode::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "Mic node activated!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn MicNode::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "Mic node deactivated!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn MicNode::on_shutdown(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "Mic node shuttind down!"); + cleanup(); + return CallbackReturn::SUCCESS; } void MicNode::configure_microphone() { @@ -138,7 +162,7 @@ int main(int argc, char* argv[]) { auto node = std::make_shared(rclcpp::NodeOptions()); rclcpp::executors::StaticSingleThreadedExecutor executor; - executor.add_node(node); + executor.add_node(node->get_node_base_interface()); executor.spin(); rclcpp::shutdown(); diff --git a/rae_hw/src/speakers_node.cpp b/rae_hw/src/peripherals/speakers.cpp similarity index 69% rename from rae_hw/src/speakers_node.cpp rename to rae_hw/src/peripherals/speakers.cpp index 3f72f92..55c3dbe 100644 --- a/rae_hw/src/speakers_node.cpp +++ b/rae_hw/src/peripherals/speakers.cpp @@ -2,27 +2,49 @@ namespace rae_hw { -SpeakersNode::SpeakersNode(const rclcpp::NodeOptions& options) : Node("speakers_node", options) { +SpeakersNode::SpeakersNode(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("speakers_node", options) {} + +SpeakersNode::~SpeakersNode() { + cleanup(); +} + +void SpeakersNode::cleanup() { + delete[] buffer; + mpg123_close(mh); + mpg123_delete(mh); + mpg123_exit(); +} + +CallbackReturn SpeakersNode::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) { // Initialize ALSA or any other audio setup code here // Create Audio Service play_audio_service_ = create_service( - "play_audio", std::bind(&SpeakersNode::play_audio_service_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + "play_audio", std::bind(&SpeakersNode::play_audio_service_callback, this, std::placeholders::_1, std::placeholders::_2)); // Any other initialization code here - RCLCPP_INFO(this->get_logger(), "Speakers node running!"); + RCLCPP_INFO(this->get_logger(), "Speakers node configured!"); + return CallbackReturn::SUCCESS; } -SpeakersNode::~SpeakersNode() { - delete[] buffer; - mpg123_close(mh); - mpg123_delete(mh); - mpg123_exit(); +CallbackReturn SpeakersNode::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "Speakers node activating!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn SpeakersNode::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "Speakers node deactivating!"); + return CallbackReturn::SUCCESS; +} + +CallbackReturn SpeakersNode::on_shutdown(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(this->get_logger(), "Speakers node shutting down!"); + cleanup(); + return CallbackReturn::SUCCESS; } -void SpeakersNode::play_audio_service_callback(const std::shared_ptr request_header, - const std::shared_ptr request, +void SpeakersNode::play_audio_service_callback(const std::shared_ptr request, const std::shared_ptr response) { // Use request->mp3_file to get the MP3 file location const char* mp3_file = request->mp3_file.c_str(); @@ -38,7 +60,6 @@ void SpeakersNode::play_mp3(const char* mp3_file) { // Initialize libmpg123 mpg123_init(); mh = mpg123_new(NULL, NULL); - int mic_rate = 44100; long rate; // Set your desired sample rate here int channels, encoding; if(mpg123_open(mh, mp3_file) != MPG123_OK || mpg123_getformat(mh, &rate, &channels, &encoding) != MPG123_OK) { @@ -54,7 +75,7 @@ void SpeakersNode::play_mp3(const char* mp3_file) { // Set ALSA parameters - snd_pcm_set_params(alsaHandle, SND_PCM_FORMAT_S16_LE, SND_PCM_ACCESS_RW_INTERLEAVED, channels, mic_rate * channels, 2, 50000); + snd_pcm_set_params(alsaHandle, SND_PCM_FORMAT_S16_LE, SND_PCM_ACCESS_RW_INTERLEAVED, 1, rate * channels, 2, 50000); // Read and play MP3 file size_t buffer_size = mpg123_outblock(mh) * 4; @@ -84,7 +105,7 @@ int main(int argc, char* argv[]) { auto node = std::make_shared(rclcpp::NodeOptions()); rclcpp::executors::StaticSingleThreadedExecutor executor; - executor.add_node(node); + executor.add_node(node->get_node_base_interface()); executor.spin(); rclcpp::shutdown(); diff --git a/rae_sdk/rae_sdk/robot/api/ros/ros_interface.py b/rae_sdk/rae_sdk/robot/api/ros/ros_interface.py index a1a4129..49ff8b0 100644 --- a/rae_sdk/rae_sdk/robot/api/ros/ros_interface.py +++ b/rae_sdk/rae_sdk/robot/api/ros/ros_interface.py @@ -18,6 +18,7 @@ from launch.actions import IncludeLaunchDescription from launch import LaunchDescription, LaunchService from geometry_msgs.msg import TransformStamped +from std_msgs.msg import String from tf2_ros.transform_listener import TransformListener from tf2_ros.buffer import Buffer from tf2_ros import TransformException @@ -107,7 +108,8 @@ def __init__(self, robot_options: RobotOptions=RobotOptions()): self._timers: dict[str, Timer] = {} self._tf_buffer = None self._tf_listener = None - + self._controller_param_file = os.path.join(get_package_share_directory('rae_hw'), 'config', 'controller.yaml') + self._ready = False @property def node(self): return self._node @@ -120,13 +122,18 @@ def start_hardware_process(self): IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(get_package_share_directory('rae_hw'), 'launch', launch_name)), - launch_arguments={'namespace': self._namespace}.items()) + launch_arguments={'namespace': self._namespace, + 'controller_params_file': self._controller_param_file}.items()) ]) self._stop_event = multiprocessing.Event() self._process = multiprocessing.Process( target=self._run_process, args=(self._stop_event, ld), daemon=True) self._process.start() + # create subscribers for ready topic + self.create_subscriber('ready', String, self._ready_callback) + + def _run_process(self, stop_event, launch_description): loop = asyncio.get_event_loop() launch_service = LaunchService() @@ -146,8 +153,6 @@ def start(self) -> None: start_hardware (bool): Whether to start the hardware process or not. """ - if self._start_hardware or self._launch_mock: - self.start_hardware_process() self._context = rclpy.Context() self._context.init() log.info("ROS2 context initialized.") @@ -162,6 +167,10 @@ def start(self) -> None: self._executor_thread = threading.Thread(target=self._spin) self._executor_thread.start() log.info(f"Node started!") + if self._start_hardware or self._launch_mock: + self.start_hardware_process() + while not self._ready: + sleep(0.5) def _spin(self): log.info("rlcpy thread> Start") @@ -373,3 +382,6 @@ def get_frame_position(self, source_frame, target_frame) -> TransformStamped: except TransformException as e: log.error(e) return None + def _ready_callback(self, msg): + log.info(f"Received ready message: {msg}") + self._ready = True \ No newline at end of file