ROS1-ROS2 Bridge Demo¶
Introduction¶
This is a system integration exercise to demonstrate operation of the ROS1-ROS2 topic and service
bridge. Using the bridge does not require new techniques when developing either ROS1 or ROS2
software, so much of the required code is provided to you in a template workspace (~/industrial_training/exercises/7.2/template_ws/src
). Instead, we will focus on developing code that interfaces with the ros1_bridge
package.
In this exercise, the ROS1 ARMarker
message, which is the goal destination for the robot, must be communicated to the ROS2 application. Then, the ROS2 application needs to use service clients to call a planner that only
exists as a ROS1 package. Specifically, this exercise calls the Descartes motion planner
as seen in Exercise 4.1. On the ROS1 side, service servers are provided to generate motion plans and to
execute them. In order to use these custom services, the bridge needs to be compiled from source
with the message definitions in the build environment, which is the bulk of the complexity in this
exercise.
Building the Demo¶
This exercise uses the ROS1 bridge to call ROS1 nodes from ROS2 nodes and therefore the build procedure is somewhat involved.
Create a ROS1 workspace from Exercise 4.1¶
Open a new terminal. Create a catkin workspace for the exercise 4.1 ROS1 packages and dependencies.
mkdir -p ~/catkin_ws_to_bridge/src
Copy Exercise 4.1 in the training repo.
cp -r ~/industrial_training/exercises/4.1/ros1/src ~/catkin_ws_to_bridge/src/demo
Clone additional dependencies.
cd ~/catkin_ws_to_bridge/src git clone --branch melodic-devel https://github.com/ros-industrial-consortium/descartes.git git clone --branch kinetic-devel https://github.com/ros-industrial/universal_robot.git git clone --branch master https://github.com/ros-industrial/fake_ar_publisher.git
Source and build exercise from the top workspace directory.
source /opt/ros/noetic/setup.bash cd ~/catkin_ws_to_bridge catkin build
From the above steps, the ROS1 catkin workspace is being provided without requiring edits. Take note of the critical components that will be communicated over the bridge.
There is an
ARMarker
topic message (directory~/catkin_ws_to_bridge/src/fake_ar_publisher/msg
) published by the node withinfake_ar_publisher.cpp
(directory~/catkin_ws_to_bridge/src/fake_ar_publisher/src
).Additionally, there are service messages (directory
~/catkin_ws_to_bridge/src/demo/myworkcell_core/srv
), and the service servers will be adverstized by nodes (directory~/catkin_ws_to_bridge/src/demo/myworkcell_core/src/
).Service message
LocalizePart
advertised by node withinvision_node.cpp
Service message
PlanCartesianPath
advertised by node withindescartes_node.cpp
Service message
ExecuteTrajectory
advertised by node withinmove_robot.cpp
Service message
MoveToPose
advertised by node withinmove_robot.cpp
Create the ROS2 workspace¶
In a new terminal, create a colcon workspace for the exercise 7.2 ROS1 packages and dependencies. We will copy a template with part of the exercise created for you.
mkdir -p ~/colcon_ws/src cd ~/colcon_ws/src
cp -r ~/industrial_training/exercises/7.2/template_ws/src ~/colcon_ws/src/demo
Open the blank file located at the following directory:
gedit ~/colcon_ws/src/demo/myworkcell_msgs/message_mappings.yaml
Add the message mapping parameters to the blank file. This will map four services that we plan on using in our ROS2 node (
~/colcon_ws/src/demo/myworkcell_core/src/myworkcell_core.cpp
). These are necessary to call the planner in ROS1 and command the robot to execute the trajectory. Additionally, we will map one publisher/subscriber message calledARMarker
, but the ROS2 C++ code will not require this message. We will use it in the terminal in the later section called “Communicate a Publisher/subscriber topic over the bridge.” The final contents of the YAML file should be as follows:- ros1_package_name: 'myworkcell_core' ros1_service_name: 'PlanCartesianPath' ros2_package_name: 'myworkcell_msgs' ros2_service_name: 'PlanCartesianPath' - ros1_package_name: 'myworkcell_core' ros1_service_name: 'MoveToPose' ros2_package_name: 'myworkcell_msgs' ros2_service_name: 'MoveToPose' - ros1_package_name: 'myworkcell_core' ros1_service_name: 'ExecuteTrajectory' ros2_package_name: 'myworkcell_msgs' ros2_service_name: 'ExecuteTrajectory' - ros1_package_name: 'myworkcell_core' ros1_service_name: 'LocalizePart' ros2_package_name: 'myworkcell_msgs' ros2_service_name: 'LocalizePart' - ros1_package_name: 'fake_ar_publisher' ros1_message_name: 'ARMarker' ros2_package_name: 'myworkcell_msgs' ros2_message_name: 'ARMarker'
Save and close the file
~/colcon_ws/src/demo/myworkcell_msgs/message_mappings.yaml
.Open the ScanNPlan node file file at the following directory:
gedit ~/colcon_ws/src/demo/myworkcell_core/src/myworkcell_core.cpp
Add the definitions and declarations for the services that will be communicated over the bridge when calling the ROS1 planner.
class ScanNPlan: public rclcpp::Node { public: ScanNPlan(const rclcpp::NodeOptions & options): rclcpp::Node(NODE_NAME, options) { //***FILL CODE HERE IN CONSTRUCTOR FOR SERVICES vision_client_ = this->create_client<myworkcell_msgs::srv::LocalizePart>("localize_part"); cartesian_client_ = this->create_client<myworkcell_msgs::srv::PlanCartesianPath>("plan_path"); move_client_ = this->create_client<myworkcell_msgs::srv::MoveToPose>("move_to_pose"); exec_traj_client_ = this->create_client<myworkcell_msgs::srv::ExecuteTrajectory>("execute_trajectory"); } ~ScanNPlan() { } bool start(const std::string& base_frame) { //***FILL CODE HERE TO CALL THE ROS SERVICES OVER THE BRIDGE } //***FILL CODE HERE TO DECLARE SERVICES rclcpp::Client<myworkcell_msgs::srv::LocalizePart>::SharedPtr vision_client_; rclcpp::Client<myworkcell_msgs::srv::PlanCartesianPath>::SharedPtr cartesian_client_; rclcpp::Client<myworkcell_msgs::srv::MoveToPose>::SharedPtr move_client_; rclcpp::Client<myworkcell_msgs::srv::ExecuteTrajectory>::SharedPtr exec_traj_client_; };
Add a
start()
method that will call the service servers from ROS1. The final start method should appear as follows to use all four services we previously mapped.bool start(const std::string& base_frame) { //***FILL CODE HERE TO CALL THE ROS SERVICES OVER THE BRIDGE using namespace myworkcell_msgs::srv; // waiting for services std::vector<rclcpp::ClientBase* > clients = {vision_client_.get(), cartesian_client_.get() , move_client_.get(), exec_traj_client_.get()}; if(std::all_of(clients.begin(), clients.end(),[](rclcpp::ClientBase* c){ std::cout<<"Waiting for client "<< c->get_service_name()<<std::endl; return c->wait_for_service(std::chrono::seconds(WAIT_SERVICE_PERIOD)); })) { std::cout<<"Found all services"<<std::endl; } else { RCLCPP_ERROR(this->get_logger(), "The service was not found"); return false; } // sending localization request LocalizePart::Request::SharedPtr req = std::make_shared<LocalizePart::Request>(); req->base_frame = base_frame; std::cout<<"Requesting pose in base frame: "<< base_frame << std::endl; //RCLCPP_INFO(this->get_logger(),"Requesting pose in base frame: %s", base_frame.c_str()); // TODO: enable this when printing to console works again std::shared_future<LocalizePart::Response::SharedPtr> result_future = vision_client_->async_send_request(req); if(rclcpp::spin_until_future_complete(this->get_node_base_interface(),result_future) != rclcpp::executor::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(this->get_logger(), "Localize service call failed"); return false; } if(!result_future.get()->succeeded) { RCLCPP_ERROR(this->get_logger(), "Could not localize part"); return false; } // getting response LocalizePart::Response::SharedPtr res = result_future.get(); std::cout<<"Part localized"<<std::endl; geometry_msgs::msg::Pose move_target = res->pose; // moving to pose MoveToPose::Request::SharedPtr move_req = std::make_shared<MoveToPose::Request>(); move_req->pose.header.frame_id = base_frame; move_req->pose.pose = move_target; std::shared_future<MoveToPose_Response::SharedPtr> move_future = move_client_->async_send_request(move_req); if(rclcpp::spin_until_future_complete(this->get_node_base_interface(),move_future) != rclcpp::executor::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(this->get_logger(),"Failed to move to target"); return false; } // planning cartesian path PlanCartesianPath_Request::SharedPtr plan_req = std::make_shared<PlanCartesianPath::Request>(); plan_req->pose = move_target; std::cout<<"Planning trajectory"<<std::endl; std::shared_future<PlanCartesianPath_Response::SharedPtr> plan_future = cartesian_client_->async_send_request(plan_req); if(rclcpp::spin_until_future_complete(this->get_node_base_interface(),plan_future) != rclcpp::executor::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(this->get_logger(),"Could not plan for path"); return false; } // executing ExecuteTrajectory::Request::SharedPtr exec_req = std::make_shared<ExecuteTrajectory::Request>(); exec_req->trajectory = plan_future.get()->trajectory; std::cout<<"Executing trajectory"<<std::endl; std::shared_future<ExecuteTrajectory::Response::SharedPtr> exec_future = exec_traj_client_->async_send_request(exec_req); if(rclcpp::spin_until_future_complete(this->get_node_base_interface(), exec_future) != rclcpp::executor::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(this->get_logger(),"Failed to execute trajectory"); return false; } std::cout<<"Trajectory execution complete"<<std::endl; return true; }
Add a main method that starts ROS2 and calls the
start()
method after instantiation of theScanNPlan
class.int main(int argc, char** argv) { //***FILL CODE HERE TO START ROS2, INSTANTIATE THE ScanNPlan CLASS, AND CALL THE start() METHOD rclcpp::init(argc,argv); std::shared_ptr<ScanNPlan> app = std::make_shared<ScanNPlan>(rclcpp::NodeOptions()); // getting parameter rclcpp::ParameterValue base_frame_param = app->declare_parameter("base_frame"); if(base_frame_param.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { RCLCPP_ERROR(app->get_logger(),"Parameter not found"); return -1; } std::string base_frame = base_frame_param.get<std::string>(); std::cout<<"Got base_frame parameter " << base_frame << std::endl; if(!app->start(base_frame)) { rclcpp::shutdown(); return -1; } rclcpp::shutdown(); return 0; }
Save and close the file
~/colcon_ws/src/demo/myworkcell_core/src/myworkcell_core.cpp
.Build the ROS2 workspace.
cd ~/colcon_ws source /opt/ros/foxy/setup.bash colcon build
Build the ROS1 Bridge¶
If you have ros2_control
installed, you may encounter a known error while building the ros1_bridge
(#329). This can be avoided by purging controller_manager_msgs
from the foxy
installation.
sudo apt purge ros-foxy-controller-manager-msgs --autoremove
At the end of Exercise 7.2, the last instruction will remind you to reinstall ros2_control
so the package can be used during other demos and exercises.
Open a new terminal. Create the ros1_bridge workspace. We build the bridge in a separate workspace because it needs to see both ROS1 and ROS2 packages in its environment, and we want to make sure our application workspaces only see the packages from the distribution they are in.
mkdir -p ~/ros1_bridge_ws/src cd ~/ros1_bridge_ws/src
Clone the ROS1 bridge into your ROS2 workspace, selecting the branch that matches your ROS release.
git clone -b foxy https://github.com/ros2/ros1_bridge.git
Source ROS1 and ROS2 setup bash files. This is one of the only times you’ll want to mix setup files from different ROS distributions.
source ~/catkin_ws_to_bridge/devel/setup.bash source ~/colcon_ws/install/setup.bash
You should see a warning about the
ROS_DISTRO
variable being previously set to a different value. Now the environment contains references to both distributions which can be verified by observing the CMake path.echo $CMAKE_PREFIX_PATH | tr ':' '\n'
Build the bridge. This may take a while (~30 min) since it is creating mappings between all known message and service types.
cd ~/ros1_bridge_ws colcon build --packages-select ros1_bridge --cmake-force-configure --cmake-args -DBUILD_TESTING=FALSE
List the mapped message and service pairs.
source install/local_setup.bash ros2 run ros1_bridge dynamic_bridge --print-pairs
This should list the custom mapped services.
'myworkcell_msgs/srv/ExecuteTrajectory' (ROS 2) <=> 'myworkcell_core/ExecuteTrajectory' (ROS 1)
'myworkcell_msgs/srv/LocalizePart' (ROS 2) <=> 'myworkcell_core/LocalizePart' (ROS 1)
'myworkcell_msgs/srv/MoveToPose' (ROS 2) <=> 'myworkcell_core/MoveToPose' (ROS 1)
'myworkcell_msgs/srv/PlanCartesianPath' (ROS 2) <=> 'myworkcell_core/PlanCartesianPath' (ROS 1)
The command should also list the custom mapped topic message.
'myworkcell_msgs/msg/ARMarker' (ROS 2) <=> 'fake_ar_publisher/ARMarker' (ROS 1)
If you struggle to verify this information in the lengthy terminal output, use
grep
to simplify the terminal output.ros2 run ros1_bridge dynamic_bridge --print-pairs | grep myworkcell
Run the Demo¶
Run the ROS1 nodes¶
Open a new terminal, source your ROS1 workspace, and run the following launch file.
cd ~/catkin_ws_to_bridge source devel/setup.bash roslaunch myworkcell_support ros2_setup.launch
This will launch the motion planning and motion execution nodes is ROS1. Rviz will come up as well.
Run the ROS1 bridge¶
In your ros1_bridge_ws ROS2 workspace, source the workspace if you have not already.
cd ~/ros1_bridge_ws source install/setup.bash
Export the ROS_MASTER_URI environment variable.
export ROS_MASTER_URI=http://localhost:11311
Run the bridge.
ros2 run ros1_bridge dynamic_bridge --bridge-all-topics
We are using
--bridge-all-topics
argument to ensure thedynamic_bridge
will force a connnection to be made, even for topics not used in the ROS2 C++ code. This allows a ROS2 terminal to view all of the ROS1 topics. For example, without the--bridge-all-topics
argument, a mapped message topic likear_pose_marker
would not be bridged since there is not a ROS2 subscriber in in the C++ code.
Run the ROS2 nodes¶
Open a new terminal and source your ROS2 workspace.
cd ~/colcon_ws source install/setup.bash
Run the python launch file that starts the ROS2 nodes for this application.
ros2 launch myworkcell_core workcell.launch.py
If the program succeeds you should see the following output:
[INFO] [myworkcell_node-1]: process started with pid [522184]
[myworkcell_node-1] Got base_frame parameter world
[myworkcell_node-1] Waiting for client /localize_part
[myworkcell_node-1] Waiting for client /plan_path
[myworkcell_node-1] Waiting for client /move_to_pose
[myworkcell_node-1] Waiting for client /execute_trajectory
[myworkcell_node-1] Found all services
[myworkcell_node-1] Requesting pose in base frame: world
[myworkcell_node-1] Part localized
[myworkcell_node-1] Planning trajectory
[myworkcell_node-1] Executing trajectory
[myworkcell_node-1] Trajectory execution complete
[INFO] [myworkcell_node-1]: process has finished cleanly [pid 522184]
You should also see the robot moving accordingly in the Rviz window.
Communicate a Publisher/subscriber topic over the bridge:¶
The information for the ARMarker message is already communicated over the bridge using the ROS1 Service Server localize_part
. However, we will practice using a custom topic message to confirm the information by subscribing to the ar_pose_marker
topic from the terminal.
Open a new terminal and source to the ROS2 workspace.
source ~/colcon_ws/install/setup.bash
List the available ROS2 topics. You should see ar_pose_marker
among the options.
ros2 topic list
Print the topic’s messages in your terminal.
ros2 topic echo /ar_pose_marker
Displayed in the terminal, you should see the marker goal pose that was used for motion planning. The ROS2 workspace called the planner in the previous section called “Run the ROS2 nodes.”
REMINDER: If purging controller_manager_msgs
from the foxy
installation was required to complete Exercise 7.2, reinstall ros2_control
so the package can be used during other demos and exercises.
sudo apt install ros-foxy-ros2-control