Skip to content

Commit 228a7b2

Browse files
Demonmasterlqxkscottzchristophebedardahcorde
authored
add cpp version for urdf_tutorial (#4926)
* Update Debugging-Tf2-Problems.rst some command to launch debug demo is wrong and correct it Signed-off-by: Demonmasterlqx <[email protected]> * Update Debugging-Tf2-Problems.rst Signed-off-by: Demonmasterlqx <[email protected]> * Rename Using-URDF-with-Robot-State-Publisher.rst to Using-URDF-with-Robot-State-Publisher-py.rst change name Signed-off-by: Demonmasterlqx <[email protected]> * Create Using-URDF-with-Robot-State-Publisher-cpp.rst add cpp version Signed-off-by: Demonmasterlqx <[email protected]> * Update Using-URDF-with-Robot-State-Publisher-cpp.rst add .. redirect-from:: Tutorials/URDF/Using-URDF-with-Robot-State-Publisher Signed-off-by: Demonmasterlqx <[email protected]> * Update Using-URDF-with-Robot-State-Publisher-cpp.rst Signed-off-by: Demonmasterlqx <[email protected]> * Update Using-URDF-with-Robot-State-Publisher-cpp.rst Polish my language Signed-off-by: Demonmasterlqx <[email protected]> * Update Using-URDF-with-Robot-State-Publisher-cpp.rst add code comment Signed-off-by: Demonmasterlqx <[email protected]> * Update Using-URDF-with-Robot-State-Publisher-cpp.rst fix format problems Signed-off-by: Demonmasterlqx <[email protected]> * White space help. * Update source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-cpp.rst Signed-off-by: Katherine Scott <[email protected]> * Update source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-cpp.rst Signed-off-by: Katherine Scott <[email protected]> * Update source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-cpp.rst Signed-off-by: Katherine Scott <[email protected]> * Update source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-cpp.rst Signed-off-by: Katherine Scott <[email protected]> * Update source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-cpp.rst Signed-off-by: Katherine Scott <[email protected]> * Update source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-cpp.rst Signed-off-by: Katherine Scott <[email protected]> * Update source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-cpp.rst Co-authored-by: Christophe Bedard <[email protected]> Signed-off-by: Demonmasterlqx <[email protected]> * Update source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-cpp.rst Co-authored-by: Christophe Bedard <[email protected]> Signed-off-by: Demonmasterlqx <[email protected]> * codespell Signed-off-by: Alejandro Hernandez Cordero <[email protected]> * Title underline too short Signed-off-by: Alejandro Hernandez Cordero <[email protected]> * make tutorials visible Signed-off-by: Alejandro Hernandez Cordero <[email protected]> --------- Signed-off-by: Demonmasterlqx <[email protected]> Signed-off-by: Katherine Scott <[email protected]> Signed-off-by: Alejandro Hernandez Cordero <[email protected]> Co-authored-by: kscottz <[email protected]> Co-authored-by: Christophe Bedard <[email protected]> Co-authored-by: Alejandro Hernandez Cordero <[email protected]>
1 parent f5e2a68 commit 228a7b2

File tree

3 files changed

+369
-4
lines changed

3 files changed

+369
-4
lines changed

‎source/Tutorials/Intermediate/URDF/URDF-Main.rst‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,5 +20,6 @@ URDF (Unified Robot Description Format) is a file format for specifying the geom
2020
Building-a-Movable-Robot-Model-with-URDF
2121
Adding-Physical-and-Collision-Properties-to-a-URDF-Model
2222
Using-Xacro-to-Clean-Up-a-URDF-File
23-
Using-URDF-with-Robot-State-Publisher
23+
Using-URDF-with-Robot-State-Publisher-cpp
24+
Using-URDF-with-Robot-State-Publisher-py
2425
Exporting-an-URDF-File

‎source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-cpp.rst‎

Lines changed: 364 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,364 @@
1+
.. Redirect-from::
2+
3+
Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher
4+
5+
.. _URDFPlusRSPCPP:
6+
7+
Using URDF with ``robot_state_publisher`` in CPP
8+
================================================
9+
10+
**Goal:** Simulate a walking robot modeled in URDF and view it in Rviz.
11+
12+
**Tutorial level:** Intermediate
13+
14+
**Time:** 15 minutes
15+
16+
.. contents:: Contents
17+
:depth: 2
18+
:local:
19+
20+
Background
21+
----------
22+
23+
This tutorial will show you how to model a walking robot, publish the state as a tf2 message and view the simulation in Rviz.
24+
First, we create the URDF model describing the robot assembly.
25+
Next we write a node which simulates the motion and publishes the JointState and transforms.
26+
We then use ``robot_state_publisher`` to publish the entire robot state to ``/tf2``.
27+
28+
.. image:: images/r2d2_rviz_demo.gif
29+
30+
Prerequisites
31+
-------------
32+
33+
- `rviz2 <https://index.ros.org/p/rviz2/>`__
34+
35+
As always, don’t forget to source ROS 2 in :doc:`every new terminal you open <../../Beginner-CLI-Tools/Configuring-ROS2-Environment>`.
36+
37+
Tasks
38+
-----
39+
40+
1 Create a package
41+
^^^^^^^^^^^^^^^^^^
42+
43+
Go to your ROS 2 workplace and create a package names ``urdf_tutorial_cpp``:
44+
45+
.. code-block:: console
46+
47+
cd src
48+
ros2 pkg create --build-type ament_python --license Apache-2.0 urdf_tutorial_cpp --dependencies rclpy
49+
cd urdf_tutorial_cpp
50+
51+
You should now see a ``urdf_tutorial_cpp`` folder.
52+
Next you will make several changes to it.
53+
54+
2 Create the URDF File
55+
^^^^^^^^^^^^^^^^^^^^^^
56+
57+
Create the directory where we will store some assets:
58+
59+
.. tabs::
60+
61+
.. group-tab:: Linux
62+
63+
.. code-block:: console
64+
65+
mkdir -p urdf
66+
67+
.. group-tab:: macOS
68+
69+
.. code-block:: console
70+
71+
mkdir -p urdf
72+
73+
.. group-tab:: Windows
74+
75+
.. code-block:: console
76+
77+
md urdf
78+
79+
Download the :download:`URDF file <documents/r2d2.urdf.xml>` and save it as ``urdf_tutorial_cpp/urdf/r2d2.urdf.xml``.
80+
Download the :download:`Rviz configuration file <documents/r2d2.rviz>` and save it as ``urdf_tutorial_cpp/urdf/r2d2.rviz``.
81+
82+
3 Publish the state
83+
^^^^^^^^^^^^^^^^^^^
84+
85+
Now we need a method for specifying what state the robot is in.
86+
87+
To do this, we must specify all three joints and the overall robot geometry.
88+
89+
Fire up your favorite editor and paste the following code into
90+
91+
``urdf_tutorial_cpp/src/urdf_tutorial.cpp``
92+
93+
.. code-block:: cpp
94+
95+
#include <rclcpp/rclcpp.hpp>
96+
#include <geometry_msgs/msg/quaternion.hpp>
97+
#include <sensor_msgs/msg/joint_state.hpp>
98+
#include <tf2_ros/transform_broadcaster.h>
99+
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
100+
#include <cmath>
101+
#include <thread>
102+
#include <chrono>
103+
104+
using namespace std::chrono;
105+
106+
class StatePublisher : public rclcpp::Node{
107+
public:
108+
109+
StatePublisher(rclcpp::NodeOptions options=rclcpp::NodeOptions()):
110+
Node("state_publisher",options){
111+
joint_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states",10);
112+
// create a publisher to tell robot_state_publisher the JointState information.
113+
// robot_state_publisher will deal with this transformation
114+
broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
115+
// create a broadcaster to tell the tf2 state information
116+
// this broadcaster will determine the position of coordinate system 'asix' in coordinate system 'odom'
117+
RCLCPP_INFO(this->get_logger(),"Starting state publisher");
118+
119+
loop_rate_=std::make_shared<rclcpp::Rate>(33ms);
120+
121+
timer_=this->create_wall_timer(33ms,std::bind(&StatePublisher::publish,this));
122+
}
123+
124+
void publish();
125+
private:
126+
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
127+
std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster;
128+
rclcpp::Rate::SharedPtr loop_rate_;
129+
rclcpp::TimerBase::SharedPtr timer_;
130+
131+
//Robot state variables
132+
// degree means one degree
133+
const double degree=M_PI/180.0;
134+
double tilt = 0.;
135+
double tinc = degree;
136+
double swivel = 0.;
137+
double angle = 0.;
138+
double height = 0.;
139+
double hinc = 0.005;
140+
};
141+
142+
void StatePublisher::publish(){
143+
// create the necessary messages
144+
geometry_msgs::msg::TransformStamped t;
145+
sensor_msgs::msg::JointState joint_state;
146+
147+
// add time stamp
148+
joint_state.header.stamp=this->get_clock()->now();
149+
// Specify joints' name which are defined in the r2d2.urdf.xml and their content
150+
joint_state.name={"swivel","tilt","periscope"};
151+
joint_state.position={swivel,tilt,height};
152+
153+
// add time stamp
154+
t.header.stamp=this->get_clock()->now();
155+
// specify the father and child frame
156+
157+
// odom is the base coordinate system of tf2
158+
t.header.frame_id="odom";
159+
// axis is defined in r2d2.urdf.xml file and it is the base coordinate of model
160+
t.child_frame_id="axis";
161+
162+
// add translation change
163+
t.transform.translation.x=cos(angle)*2;
164+
t.transform.translation.y=sin(angle)*2;
165+
t.transform.translation.z=0.7;
166+
tf2::Quaternion q;
167+
// euler angle into Quanternion and add rotation change
168+
q.setRPY(0,0,angle+M_PI/2);
169+
t.transform.rotation.x=q.x();
170+
t.transform.rotation.y=q.y();
171+
t.transform.rotation.z=q.z();
172+
t.transform.rotation.w=q.w();
173+
174+
// update state for next time
175+
tilt+=tinc;
176+
if (tilt<-0.5 || tilt>0.0){
177+
tinc*=-1;
178+
}
179+
height+=hinc;
180+
if (height>0.2 || height<0.0){
181+
hinc*=-1;
182+
}
183+
swivel+=degree; // Increment by 1 degree (in radians)
184+
angle+=degree; // Change angle at a slower pace
185+
186+
// send message
187+
broadcaster->sendTransform(t);
188+
joint_pub_->publish(joint_state);
189+
190+
RCLCPP_INFO(this->get_logger(),"Publishing joint state");
191+
}
192+
193+
int main(int argc, char * argv[]){
194+
rclcpp::init(argc,argv);
195+
rclcpp::spin(std::make_shared<StatePublisher>());
196+
rclcpp::shutdown();
197+
return 0;
198+
}
199+
200+
This file will send ``joint_state`` values to ``robot_state_publisher`` which in turn will tell tf2 how to place model.
201+
202+
The code file will also tell ``tf2`` how to place the whole model using the ``transform_broadcaster``
203+
204+
4 Create a launch file
205+
^^^^^^^^^^^^^^^^^^^^^^
206+
207+
Create a new ``urdf_tutorial_cpp/launch`` folder.
208+
Open your editor and paste the following code, saving it as ``urdf_tutorial_cpp/launch/launch.py``
209+
210+
.. code-block:: python
211+
212+
import os
213+
from ament_index_python.packages import get_package_share_directory
214+
from launch import LaunchDescription
215+
from launch.actions import DeclareLaunchArgument
216+
from launch.substitutions import LaunchConfiguration
217+
from launch_ros.actions import Node
218+
219+
def generate_launch_description():
220+
221+
# ''use_sim_time'' is used to determine whether ros2 use simulation time provided by simulation environment (Gazebo).
222+
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
223+
224+
urdf_file_name = 'r2d2.urdf.xml'
225+
226+
# urdf is path of urdf_file_name file. we use define it as the follow due to the CMakeLists.txt file.
227+
urdf=os.path.join(
228+
get_package_share_directory('urdf_tutorial_cpp'),
229+
'urdf',
230+
urdf_file_name
231+
)
232+
233+
# open the whole urdf_file_name file and read it content to robot_desc
234+
with open(urdf, 'r') as infp:
235+
robot_desc = infp.read()
236+
237+
return LaunchDescription([
238+
DeclareLaunchArgument(
239+
'use_sim_time',
240+
default_value='false',
241+
description='Use simulation (Gazebo) clock if true'),
242+
Node(
243+
package='robot_state_publisher',
244+
executable='robot_state_publisher',
245+
name='robot_state_publisher',
246+
output='screen',
247+
parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
248+
arguments=[urdf]),
249+
Node(
250+
package='urdf_tutorial_cpp',
251+
executable='urdf_tutorial_cpp',
252+
name='urdf_tutorial_cpp',
253+
output='screen'),
254+
])
255+
256+
257+
5 Edit the CMakeLists.txt file
258+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
259+
260+
You must tell the **colcon** build tool how to install your cpp package.
261+
Edit the ``CMakeLists.txt`` file as follows:
262+
263+
.. code-block:: cmake
264+
265+
cmake_minimum_required(VERSION 3.8)
266+
project(urdf_tutorial_cpp)
267+
268+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
269+
add_compile_options(-Wall -Wextra -Wpedantic)
270+
endif()
271+
272+
# find dependencies
273+
find_package(ament_cmake REQUIRED)
274+
find_package(geometry_msgs REQUIRED)
275+
find_package(sensor_msgs REQUIRED)
276+
find_package(tf2_ros REQUIRED)
277+
find_package(tf2_geometry_msgs REQUIRED)
278+
find_package(rclcpp REQUIRED)
279+
280+
add_executable(urdf_tutorial_cpp src/urdf_tutorial.cpp)
281+
282+
ament_target_dependencies(urdf_tutorial_cpp
283+
geometry_msgs
284+
sensor_msgs
285+
tf2_ros
286+
tf2_geometry_msgs
287+
rclcpp
288+
)
289+
290+
install(TARGETS
291+
urdf_tutorial_cpp
292+
DESTINATION lib/${PROJECT_NAME}
293+
)
294+
295+
install(DIRECTORY
296+
launch
297+
DESTINATION share/${PROJECT_NAME}
298+
)
299+
300+
install(DIRECTORY
301+
urdf
302+
DESTINATION share/${PROJECT_NAME}
303+
)
304+
305+
ament_package()
306+
307+
we use ``install`` command to put the r2d2.rviz into ``install`` dir
308+
309+
6 Install the package
310+
^^^^^^^^^^^^^^^^^^^^^
311+
312+
To visualize the results you will need to open a new terminal and run RViz using your RViz configuration file.
313+
314+
.. code-block:: console
315+
316+
colcon build --symlink-install --packages-select urdf_tutorial_cpp
317+
318+
Source the setup files:
319+
320+
.. tabs::
321+
322+
.. group-tab:: Linux
323+
324+
.. code-block:: console
325+
326+
source install/setup.bash
327+
328+
.. group-tab:: macOS
329+
330+
.. code-block:: console
331+
332+
source install/setup.bash
333+
334+
.. group-tab:: Windows
335+
336+
.. code-block:: console
337+
338+
call install/setup.bat
339+
340+
341+
7 View the results
342+
^^^^^^^^^^^^^^^^^^
343+
344+
To launch your new package run the following command:
345+
346+
.. code-block:: console
347+
348+
ros2 launch urdf_tutorial_cpp launch.py
349+
350+
To visualize your results you will need to open a new terminal and run Rviz using your rviz configuration file.
351+
352+
.. code-block:: console
353+
354+
rviz2 -d install/urdf_tutorial_cpp/share/urdf_tutorial_cpp/urdf/r2d2.rviz
355+
356+
See the `User Guide <http://wiki.ros.org/rviz/UserGuide>`__ for details on how to use Rviz.
357+
358+
``install/urdf_tutorial_cpp/share/urdf_tutorial_cpp/urdf/r2d2.rviz`` is the dir where the r2d2.rviz stored.
359+
360+
Summary
361+
-------
362+
363+
Congratulations!
364+
You have created a ``JointState`` publisher node and coupled it with ``robot_state_publisher`` to simulate a walking robot.

‎source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher.rst‎ renamed to ‎source/Tutorials/Intermediate/URDF/Using-URDF-with-Robot-State-Publisher-py.rst‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,10 @@
22

33
Tutorials/URDF/Using-URDF-with-Robot-State-Publisher
44

5-
.. _URDFPlusRSP:
5+
.. _URDFPlusRSPPYTHON:
66

7-
Using URDF with ``robot_state_publisher``
8-
=========================================
7+
Using URDF with ``robot_state_publisher`` in Python
8+
===================================================
99

1010
**Goal:** Simulate a walking robot modeled in URDF and view it in Rviz.
1111

0 commit comments

Comments
 (0)