Offboard control using ROS2

PepMS · July 1, 2020, 8:11pm

Hi everyone,

I’m opening this as a kind of walkthrough about how to enter offboard mode using ROS2 and the FastRTPS bridge. It might be used as an starting point for somebody trying to start working with ROS2. The example is not fully working since a failsafe is triggered as soon as you change to offboard mode.

You’ll find the ROS2 node code example here.

I’ll try to do a walk-through of the procedure to get it done. This is the problem we’ll try to solve:

Enter offboard control mode, by publishing offboard_control_mode messages at 10Hz.
Subscribe the vehicle_control_mode topic and print something when we are in the offboard mode.

PX4 side: Generate the needed FastRTPS messages

Modify the file firmware->msg->uorb_rtps_message_ids.yaml by adding the line receive: true to msg: offboard_control_mode. Similarly, add send: true to the message vehicle_control_mode.
As we are gonna test this with the SITL version compile the PX4 as follows: make px4_sitl_rtps. If you’ve compiled this version before with an old version of the yaml file you’ll probably have to do make px4_sitl_rtps clean first.

Now, the FastRTPS module inside px4 will know that it can receive offboard_control_mode messages.

ROS2 side:

Change the file ros2_ws->templates->uorb_rtps_messages_ids.yaml by adding the line receive: true to the message OffboardControlMode. Similarly, add the line send: true to the message VehicleControlMode.
Clone the repo with the ROS2 node at the specified branch in the src directory of your ROS2 workspace: git clone --single-branch --branch px4_ros2_offboard https://github.com/PepMS/px4_mpc.git
Build your workspace:
3.1 You can do it by using the px4_ros_com scripts: ./src/px4_ros_com/scripts/build_ros2_workspace.bash
3.2 You can also do colcon build

Run the simulation

In a terminal run PX4: make px4_sitl_rtps gazebo
1.1. Execute the rtps client module inside px4: micrortps_client start -t UDP
In a separate terminal, start the FastRTPS Agent: ./install/px4_ros_com/bin/micrortps_agent -t UDP
In a separate terminal you should see how the VehicleControlMode is being published:
3.1. ros2 topic echo /VehicleControlMode_PubSubTopic
3.2. Before doing the above instruction make sure you’ve sourced the file ros2_ws/install/setup.bash
Now it’s time to run our node. In a separate terminal, inside the ros2_ws do ./build/px4_mpc/offboard_controller
4.1. You can check that the offboard messages are being published by doing in a separate: ros2 topic echo /VehicleControlMode_PubSubTopic.
4.2. You can check in the PX4 that the messages are being received at 10Hz by doing: uorb top
4.3. You can switch now to offboard mode using the commander or QGC.

Something’s wrong

You’ll see that the offboard control gets activated but a failsafe is immetidately triggered saying: WARN [mc_pos_control] Offboard activation failed with error: Activation Failed. Probably I should publish some other messages like a position setpoint or something but I certainly don’t know what.

Tried this with PX4 at: 743898d5748da7d811308619c517c9e4c0701585

agent-nelson · July 22, 2020, 6:26pm

Were you able to get this working?

PepMS · July 23, 2020, 4:28am

I haven’t tried it any more. However, I think that the problem is that I wasn’t publishing any waypoint before changing the flight mode to offboard mode.

Quijotengineer · September 1, 2020, 8:22am

Hi @PepMS, have you got any updates regarding this project?
Quick question: should we clone your repo into the px4_ros_com_ros2/src directory or elsewhere?
(The build fails with error: no matching function for call to ‘rclcpp::Node::create_subscription<px4_msgs::msg::VehicleOdometry> )
Any help would we welcome. Thanks in advance.

PepMS · September 2, 2020, 5:24am

Hi @Quijotengineer, it’s been a while since I don’t take a look to this code. I needed that for my research but then other things came up on top of the priority list.

Regarding your problem, this means that your message header files have not been generated. Have you checked that you have this message marked as to be send in uorb_rtps_message_ids.yaml? I think it is the message with the id 99.

The repo shouldn’t be cloned inside another package like px4_ros_com_ros2/src. It has to be clones inside the src directory of the ros workspace as another package. It has to be at the same level as the px4_ros_com.

Hope that helps.

erik · September 2, 2020, 6:18am

I don’t know if it’s any different with ROS2, but I use ROS and mavros. When you want to switch to offboard mode you have to stream 100 setpoints before switching to offboard. Quote from the docs:

Before entering Offboard mode, you must have already started streaming setpoints. Otherwise the mode switch will be rejected. Here, 100 was chosen as an arbitrary amount.

PepMS · September 2, 2020, 7:10am

yep, that is what is probably happening! Thanks @erik! I’ll try it out when I have a moment

Quijotengineer · September 2, 2020, 10:22am

Hi @PepMS, I have that message marked as send in the uorb_rtps_message_ids.yaml file both in Firmware/msg/tools and px4_ros_com_ros2/src/px4_ros_com/templates (this workspace was created following the px4 dev guide documentation on the RTPS/ROS2 interface). I was cloning your repository at the same level as px4_ros_com in the px4_ros_com_ros2/src directory with the following structure:

px4_ros_com_ros2/
    - build/
    - install/
    - log/
    - src/
       + px4_mpc
       + px4_msgs
       + px4_ros_com

But when I build using the build_ros2_workspace.bash, it fails with:

--- stderr: px4_mpc                                                                             
/home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp: In constructor ‘OffboardControl::OffboardControl(rclcpp::Node::SharedPtr)’:
/home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:7:184: error: no matching function for call to ‘rclcpp::Node::create_subscription<px4_msgs::msg::VehicleOdometry>(const char [28], std::_Bind_helper<false, void (OffboardControl::*)(std::unique_ptr<px4_msgs::msg::VehicleOdometry_<std::allocator<void> >, std::default_delete<px4_msgs::msg::VehicleOdometry_<std::allocator<void> > > >), OffboardControl*, const std::_Placeholder<1>&>::type)’
    7 |   odometry_subs_ = node_->create_subscription<px4_msgs::msg::VehicleOdometry>("VehicleOdometry_PubSubTopic", std::bind(&OffboardControl::odometryCallback, this, std::placeholders::_1));
      |                                                                                                                                                                                        ^
In file included from /opt/ros/foxy/include/rclcpp/executors/single_threaded_executor.hpp:28,
                 from /opt/ros/foxy/include/rclcpp/executors.hpp:22,
                 from /opt/ros/foxy/include/rclcpp/rclcpp.hpp:146,
                 from /home/user/px4_ros_com_ros2/src/px4_mpc/include/px4_mpc/offboard_control.hpp:4,
                 from /home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:1:
/opt/ros/foxy/include/rclcpp/node.hpp:213:3: note: candidate: ‘template<class MessageT, class CallbackT, class AllocatorT, class CallbackMessageT, class SubscriptionT, class MessageMemoryStrategyT> std::shared_ptr<SubscriptionT> rclcpp::Node::create_subscription(const string&, const rclcpp::QoS&, CallbackT&&, const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>&, typename MessageMemoryStrategyT::SharedPtr)’
  213 |   create_subscription(
      |   ^~~~~~~~~~~~~~~~~~~
/opt/ros/foxy/include/rclcpp/node.hpp:213:3: note:   template argument deduction/substitution failed:
/home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:7:184: note:   candidate expects 5 arguments, 2 provided
    7 |   odometry_subs_ = node_->create_subscription<px4_msgs::msg::VehicleOdometry>("VehicleOdometry_PubSubTopic", std::bind(&OffboardControl::odometryCallback, this, std::placeholders::_1));
      |                                                                                                                                                                                        ^
/home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:8:198: error: no matching function for call to ‘rclcpp::Node::create_subscription<px4_msgs::msg::VehicleControlMode>(const char [31], std::_Bind_helper<false, void (OffboardControl::*)(std::unique_ptr<px4_msgs::msg::VehicleControlMode_<std::allocator<void> >, std::default_delete<px4_msgs::msg::VehicleControlMode_<std::allocator<void> > > >), OffboardControl*, const std::_Placeholder<1>&>::type)’
    8 |   ctrl_mode_subs_ = node_->create_subscription<px4_msgs::msg::VehicleControlMode>("VehicleControlMode_PubSubTopic", std::bind(&OffboardControl::vehicleCtrlModeCallback, this, std::placeholders::_1));
      |                                                                                                                                                                                                      ^
In file included from /opt/ros/foxy/include/rclcpp/executors/single_threaded_executor.hpp:28,
                 from /opt/ros/foxy/include/rclcpp/executors.hpp:22,
                 from /opt/ros/foxy/include/rclcpp/rclcpp.hpp:146,
                 from /home/user/px4_ros_com_ros2/src/px4_mpc/include/px4_mpc/offboard_control.hpp:4,
                 from /home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:1:
/opt/ros/foxy/include/rclcpp/node.hpp:213:3: note: candidate: ‘template<class MessageT, class CallbackT, class AllocatorT, class CallbackMessageT, class SubscriptionT, class MessageMemoryStrategyT> std::shared_ptr<SubscriptionT> rclcpp::Node::create_subscription(const string&, const rclcpp::QoS&, CallbackT&&, const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>&, typename MessageMemoryStrategyT::SharedPtr)’
  213 |   create_subscription(
      |   ^~~~~~~~~~~~~~~~~~~
/opt/ros/foxy/include/rclcpp/node.hpp:213:3: note:   template argument deduction/substitution failed:
/home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:8:198: note:   candidate expects 5 arguments, 2 provided
    8 |   ctrl_mode_subs_ = node_->create_subscription<px4_msgs::msg::VehicleControlMode>("VehicleControlMode_PubSubTopic", std::bind(&OffboardControl::vehicleCtrlModeCallback, this, std::placeholders::_1));
      |                                                                                                                                                                                                      ^
/home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:10:116: error: no matching function for call to ‘rclcpp::Node::create_publisher<px4_msgs::msg::OffboardControlMode>(const char [32])’
   10 |   offboard_mode_pub_ = node->create_publisher<px4_msgs::msg::OffboardControlMode>("OffboardControlMode_PubSubTopic");
      |                                                                                                                    ^
In file included from /opt/ros/foxy/include/rclcpp/executors/single_threaded_executor.hpp:28,
                 from /opt/ros/foxy/include/rclcpp/executors.hpp:22,
                 from /opt/ros/foxy/include/rclcpp/rclcpp.hpp:146,
                 from /home/user/px4_ros_com_ros2/src/px4_mpc/include/px4_mpc/offboard_control.hpp:4,
                 from /home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:1:
/opt/ros/foxy/include/rclcpp/node.hpp:184:3: note: candidate: ‘template<class MessageT, class AllocatorT, class PublisherT> std::shared_ptr<PublisherT> rclcpp::Node::create_publisher(const string&, const rclcpp::QoS&, const rclcpp::PublisherOptionsWithAllocator<AllocatorT>&)’
  184 |   create_publisher(
      |   ^~~~~~~~~~~~~~~~
/opt/ros/foxy/include/rclcpp/node.hpp:184:3: note:   template argument deduction/substitution failed:
/home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:10:116: note:   candidate expects 3 arguments, 1 provided
   10 |   offboard_mode_pub_ = node->create_publisher<px4_msgs::msg::OffboardControlMode>("OffboardControlMode_PubSubTopic");
      |                                                                                                                    ^
/home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp: In member function ‘void OffboardControl::odometryCallback(px4_msgs::msg::VehicleOdometry_<std::allocator<void> >::UniquePtr)’:
/home/user/px4_ros_com_ros2/src/px4_mpc/src/offboard_control.cpp:22:88: warning: unused parameter ‘msg’ [-Wunused-parameter]
   22 | void OffboardControl::odometryCallback(const px4_msgs::msg::VehicleOdometry::UniquePtr msg)
      |                                        ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~
make[2]: *** [CMakeFiles/offboard_controller.dir/build.make:76: CMakeFiles/offboard_controller.dir/src/offboard_control.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
make[1]: *** [CMakeFiles/Makefile2:78: CMakeFiles/offboard_controller.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< px4_mpc [3.86s, exited with code 2]
Aborted  <<< px4_ros_com [1min 39s]                                        

Summary: 1 package finished [4min 7s]
  1 package failed: px4_mpc
  1 package aborted: px4_ros_com
  2 packages had stderr output: px4_mpc px4_ros_com

Any ideas?

maponarooo · November 11, 2021, 3:31am

I have already finished compile successfuly px4_ros_com source.
Buy the way, I can’t find the “ros2_ws->templates->uorb_rtps_messages_ids.yaml” file in that directory.
What is the problem?
The example is not fully working since a failsafe is triggered as soon as I changed to offboard mode.

$ ros2 topic list
/fmu/debug_array/in
/fmu/debug_key_value/in
/fmu/timesync/in
/fmu/timesync/out
/fmu/trajectory_bezier/in
/fmu/vehicle_command/in
/fmu/vehicle_local_position_setpoint/in
/fmu/vehicle_trajectory_bezier/in
/parameter_events
/rosout
/timesync_status

~/px4_ros_com_ros2/src/px4_ros_com/scripts$ ./build_ros2_workspace.bash
openjdk version “13.0.7” 2021-04-20
OpenJDK Runtime Environment (build 13.0.7+5-Ubuntu-0ubuntu120.04)
OpenJDK 64-Bit Server VM (build 13.0.7+5-Ubuntu-0ubuntu120.04, mixed mode)
[0.682s] WARNING:colcon.colcon_core.package_selection:ignoring unknown package ‘ros1_bridge’ in --packages-skip
Starting >>> px4_msgs
Finished <<< px4_msgs [10.8s]
Starting >>> px4_ros_com
Finished <<< px4_ros_com [0.87s]

Summary: 2 packages finished [12.0s]

ROS2 workspace ready…

dsix-ls2n · January 27, 2022, 1:36pm

Hello,

Using PX4 and ROS2, we developed some applications with the RTPS bridge, working in SITL simulation and also with real drones.

We developed an alternative node to px4_ros_com, greatly inspired by it but making the micrortps_client a real ROS2 node (avoiding compatibility with several DDS protocols).

Those packages are quite recent, so there is still development going on this solution but it might help you on your own path. Do not hesitate to contact me for further information.

nandkishor · July 15, 2022, 5:13pm

I am also facing the same issue. Could you resolve this?