Skip to content

Commit a7979c0

Browse files
vcrochervcrocher
authored
ROS2 (#65)
ROS2 example implementation Author: benvonh <[email protected]>
1 parent 0b3ccf5 commit a7979c0

15 files changed

+510
-19
lines changed

‎CMakeLists.txt‎

Lines changed: 46 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -15,33 +15,40 @@ set (STATE_MACHINE_NAME "ExoTestMachine")
1515
#set (STATE_MACHINE_NAME "M2DemoMachine")
1616
#set (STATE_MACHINE_NAME "M3DemoMachine")
1717
#set (STATE_MACHINE_NAME "X2DemoMachine")
18+
#set (STATE_MACHINE_NAME "X2ROS2DemoMachine")
1819
#set (STATE_MACHINE_NAME "LoggingDevice")
1920

20-
#Use this if your state machine code folder is not in CORC 'src/apps/' folder.
21-
#Can be a relative or absolute path.
21+
# Use this if your state machine code folder is not in CORC 'src/apps/' folder.
22+
# Can be a relative or absolute path.
2223
#set (STATE_MACHINE_PATH "../")
2324

2425
# Comment to use actual hardware, uncomment for a nor robot (virtual) app
2526
set(NO_ROBOT ON)
2627

27-
# ROS Flag. set ON if you want to use ROS. Else, set OFF.
28-
set(USE_ROS OFF)
29-
# Select desired logging level (Options: TRACE, DEBUG, INFO, WARN, ERROR, CRITICAL OFF)
28+
# ROS Flag: set to 0 for a no ROS stateMachine, 1 for ROS 1 (use catkin build) and 2 for ROS2 (use colcon build)
29+
# Remember to rename select appropriate package.xml too
30+
set(ROS 0)
31+
32+
# Select desired logging level (Options: TRACE, DEBUG, INFO, WARN, ERROR, CRITICAL, OFF)
3033
# INFO is the recommended level in normal operation
3134
set(CORC_LOGGING_LEVEL INFO)
3235

3336
################################################################################
37+
38+
39+
3440
#Default path if not set
3541
if(NOT STATE_MACHINE_PATH)
3642
set (STATE_MACHINE_PATH "src/apps")
3743
endif()
3844

3945
#ROS internal flags
40-
if(USE_ROS)
41-
add_definitions(-DUSEROS)
46+
if(ROS GREATER 0)
47+
add_definitions(-DROS=${ROS})
4248
endif()
4349

44-
if(NO_ROBOT AND USE_ROS)
50+
#SIM flag is used for ROS1 simulation
51+
if(NO_ROBOT AND (ROS EQUAL 1))
4552
set(SIM ON)
4653
add_definitions(-DSIM)
4754
endif()
@@ -59,7 +66,7 @@ add_definitions(-DFP_STARTTPDO=0x3E1)
5966
## Compile as C++14
6067
set(CMAKE_CXX_STANDARD 14)
6168
set(CMAKE_CXX_STANDARD_REQUIRED ON)
62-
if(USE_ROS)
69+
if(ROS GREATER 0)
6370
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-int-in-bool-context" )
6471
else()
6572
if(CMAKE_CROSSCOMPILING)
@@ -109,16 +116,15 @@ list (APPEND INCLUDE_DIRS lib/spdlog/include/)
109116

110117
add_subdirectory(lib/yaml-cpp/)
111118

112-
113119
## Hack for Yaml files path (absolute path required for ROS use, see X2Robot::initializeRobotParams)
114120
if(CMAKE_CROSSCOMPILING)
115121
add_definitions(-DBASE_DIRECTORY=.)
116122
else()
117123
add_definitions(-DBASE_DIRECTORY=${CMAKE_SOURCE_DIR})
118124
endif()
119125

126+
if(ROS EQUAL 1)
120127
## Add ROS 1 dependencies
121-
if(USE_ROS)
122128
#ROS 1 local compile: use catkin
123129
message("--catkin--")
124130
# Required ROS packages
@@ -153,8 +159,8 @@ if(USE_ROS)
153159
)
154160

155161
generate_messages(
156-
DEPENDENCIES
157-
std_msgs
162+
DEPENDENCIES
163+
std_msgs
158164
)
159165

160166
catkin_package(
@@ -175,9 +181,20 @@ if(USE_ROS)
175181
#include CATKIN
176182
include_directories(${catkin_INCLUDE_DIRS})
177183
set(ROS_LIBRARIES ${catkin_LIBRARIES})
184+
185+
## Add ROS2 dependencies
186+
elseif(ROS EQUAL 2)
187+
#ROS 2 local compile: use colcon
188+
message("--colcon--")
189+
190+
find_package(ament_cmake REQUIRED)
191+
find_package(rclcpp REQUIRED)
192+
find_package(std_msgs REQUIRED)
193+
find_package(sensor_msgs REQUIRED)
178194
endif()
179195

180196

197+
181198
## Executable name: {STATEMACHINENAME}_APP
182199
set (APP_NAME ${STATE_MACHINE_NAME}_APP)
183200
if(NO_ROBOT)
@@ -188,7 +205,7 @@ add_executable(${APP_NAME}
188205
)
189206

190207
## Includes
191-
target_include_directories(${APP_NAME} PRIVATE ${INCLUDE_DIRS})
208+
target_include_directories(${APP_NAME} PUBLIC ${INCLUDE_DIRS})
192209

193210
## Set required external packages
194211
find_package(Threads REQUIRED)
@@ -199,12 +216,25 @@ target_link_libraries(${APP_NAME}
199216
yaml-cpp)
200217

201218
## Link ROS libraries
202-
if(USE_ROS)
219+
if(ROS EQUAL 1)
203220
target_link_libraries(${APP_NAME} ${ROS_LIBRARIES})
204221

205222
# make sure configure headers are built before any node using them
206223
add_dependencies(${APP_NAME} ${PROJECT_NAME}_gencfg)
224+
225+
elseif(ROS EQUAL 2)
226+
include_directories(${INCLUDE_DIRS})
227+
228+
ament_target_dependencies(${APP_NAME} rclcpp std_msgs sensor_msgs)
229+
ament_export_dependencies(rclcpp std_msgs sensor_msgs)
230+
231+
install(TARGETS ${APP_NAME} DESTINATION lib/${PROJECT_NAME})
232+
install(PROGRAMS script/initCAN0.sh DESTINATION lib/${PROJECT_NAME})
233+
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
234+
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
235+
236+
ament_package()
207237
endif()
208238

209239

210-
message("-----------------------------------------------\nBuilding application ${APP_NAME}\n-----------------------------------------------")
240+
message("-----------------------------------------------\nBuilding application ${APP_NAME}\n-----------------------------------------------")

‎README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@ The following individuals have made contributions to CORC:
6161
- Yue Wen
6262
- Tim Haswell
6363
- Xinliang Guo
64+
- Benjamin von Snarski
6465

6566
Please contact fong.j[at]unimelb.edu.au with questions or suggestions for continuing development, or if you wish to be more involved in the planning/organisation of CORC.
6667

‎doc/1.GettingStarted/AdvancedSimulationAndHardwareTesting.md‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,8 @@ If you would like to do a simulation:
6060
Set your state machine (X2DemoMachine):
6161
```set (STATE_MACHINE_NAME "X2DemoMachine")```
6262

63+
Rename `Package.ros1.xml` to `Package.xml` in the CORC root folder.
64+
6365
Build CORC:
6466
```bash
6567
$ cd ~/catkin_ws

‎doc/1.GettingStarted/GettingStarted.md‎

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -90,9 +90,10 @@ This example enables simple functionalities of ArmMotus M3/EMU System. It shows
9090
1. Users looking to develop for the ArmMotus M3
9191
2. Users looking to develop systems with a separate user interface
9292

93-
### [M3DemoMachine - Instructions](GSM3DemoMachine.md)
93+
### [M3DemoMachine - Instructions](GSM3DemoMachine.md)
9494
See also the code example in `src/apps/M3DemoMachine`.
9595

96+
9697
## Hardware Testing - AnkleMotus M1
9798
This example enables simple movements with the AnkleMotus M1 System.
9899

@@ -104,5 +105,20 @@ This example enables simple movements with the AnkleMotus M1 System.
104105
1. Users looking to develop for the AnkleMotus M1
105106

106107
### [M1DemoMachine - Instructions](GSM1DemoMachine.md)
107-
See the code example in `src/apps/M1DemoMachine`.
108+
See also the code example in `src/apps/M1DemoMachine`.
109+
110+
111+
## ROS2 CORC application - Exoskeleton
112+
This example is a an example of the use of a ROS2 CORC state machine.
113+
It leverages the ExoMotus X2 Exoskeleton from Fourier Intelligence, and creates a ROS2 CORC package.
114+
115+
### Requirements
116+
1. A development and deployment machine running Linux Ubuntu and ROS2 (Humble).
117+
118+
### Suggested for
119+
1. Users looking to develop a ROS2 CORC application
120+
121+
### [ROS2 Application - Instructions](ROS2Application.md)
122+
See also the code example in `src/apps/X2ROS2DemoMachine`.
123+
108124

‎doc/1.GettingStarted/ROS2Application.md‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
# ROS2 Application - Instructions
2+
3+
Minimal instructions to compile and run a CORC application as a ROS2 node able to publish the robot state.
4+
5+
This page assumes you have an Ubuntu system with ROS2 Humble installed (may work with other ROS2 versions but not tested). See [here](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) for ROS2 installation instructions.
6+
Testing the application also assumes you have an ExoMotus X2 Exoskeleton.
7+
8+
9+
> Note : these instructions are for ROS2. See [here](AdvancedSimulationAndHardwareTesting.md) for a ROS1 example.
10+
11+
## Compilation
12+
13+
14+
```bash
15+
$ source /opt/ros/humble/setup.bash
16+
```
17+
18+
In CORC root directory, rename ros2 package configuration file
19+
```bash
20+
$ mv package.ros2.xml package.xml
21+
```
22+
23+
24+
Edit the CMAKEFileList.txt to set ROS2 option and to select a ROS2 state machine:
25+
26+
```
27+
...
28+
...
29+
#set (STATE_MACHINE_NAME "ExoTestMachine")
30+
#set (STATE_MACHINE_NAME "M1DemoMachine")
31+
#set (STATE_MACHINE_NAME "M1DemoMachineROS")
32+
#set (STATE_MACHINE_NAME "M2DemoMachine")
33+
#set (STATE_MACHINE_NAME "M3DemoMachine")
34+
#set (STATE_MACHINE_NAME "X2DemoMachine")
35+
set (STATE_MACHINE_NAME "X2ROS2DemoMachine")
36+
#set (STATE_MACHINE_NAME "LoggingDevice")
37+
38+
# Use this if your state machine code folder is not in CORC 'src/apps/' folder.
39+
# Can be a relative or absolute path.
40+
#set (STATE_MACHINE_PATH "../")
41+
42+
# Comment to use actual hardware, uncomment for a nor robot (virtual) app
43+
set(NO_ROBOT OFF)
44+
45+
# ROS Flag: set to 0 for a no ROS stateMachine, 1 for ROS 1 (use catkin build) and 2 for ROS2 (use colcon build)
46+
# Remember to rename select appropriate package.xml too
47+
set(ROS 2)
48+
...
49+
...
50+
```
51+
52+
53+
From the **CANOpenController parent folder**, compile the package:
54+
```bash
55+
$ colcon build
56+
$ source install/setup.bash
57+
```
58+
59+
60+
## Run the node
61+
```bash
62+
$ ros2 run CORC X2ROS2DemoMachine_APP
63+
```
64+
65+
Here CORC is the package name and the CORC app name is the executable name.
66+
67+
See the content of the `src/apps/X2ROS2DemoMachine` folder to see how to interact with ROS2 from within CORC.

‎launch/x2_ros2.launch.py‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
import os
2+
3+
from xacro import process_file
4+
from ament_index_python import get_package_share_directory
5+
6+
from launch import LaunchDescription
7+
from launch_ros.actions import Node
8+
9+
def generate_launch_description():
10+
# Launch description
11+
ld = LaunchDescription()
12+
13+
# Package share path
14+
corc_path = get_package_share_directory("CORC")
15+
x2_description_path = get_package_share_directory("x2_description")
16+
17+
# Package share paths
18+
exo_path = os.path.join(corc_path, "config", "x2_params.yaml")
19+
rviz_path = os.path.join(x2_description_path, "rviz", "x2.rviz")
20+
21+
# Process XACRO
22+
urdf_xacro = process_file(
23+
os.path.join(x2_description_path, "urdf", "x2_fixed_base.urdf.xacro")
24+
)
25+
26+
# Nodes
27+
exo_node = Node(
28+
package="CORC",
29+
executable="X2ROS2DemoMachine_APP",
30+
arguments=["-can", "can0"],
31+
name="x2",
32+
output="screen",
33+
parameters=[
34+
{ "exo_path": exo_path }
35+
]
36+
)
37+
robot_state_node = Node(
38+
package="robot_state_publisher",
39+
executable="robot_state_publisher",
40+
name="robot_state_publisher",
41+
parameters=[
42+
{ "robot_description": urdf_xacro.toprettyxml(indent=' ') }
43+
]
44+
)
45+
rviz_node = Node(
46+
package="rviz2",
47+
executable="rviz2",
48+
arguments=["-d", rviz_path],
49+
name="rviz2"
50+
)
51+
52+
# Launch description actions
53+
ld.add_action(exo_node)
54+
ld.add_action(robot_state_node)
55+
ld.add_action(rviz_node)
56+
return ld

‎package.xml‎ renamed to ‎package.ros1.xml‎

File renamed without changes.

‎package.ros2.xml‎

Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
<?xml version="1.0"?>
2+
<package format="2">
3+
<name>CORC</name>
4+
<version>1.0.0</version>
5+
<description>The CORC package</description>
6+
7+
<!-- One maintainer tag required, multiple allowed, one person per tag -->
8+
<!-- Example: -->
9+
<!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
10+
<maintainer email="[email protected]">Benjamin von Snarski</maintainer>
11+
12+
13+
<!-- One license tag required, multiple allowed, one license per tag -->
14+
<!-- Commonly used license strings: -->
15+
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16+
<license>Apache 2</license>
17+
18+
19+
<!-- Url tags are optional, but multiple are allowed, one per tag -->
20+
<!-- Optional attribute type can be: website, bugtracker, or repository -->
21+
<!-- Example: -->
22+
<!-- <url type="website">http://wiki.ros.org/x2</url> -->
23+
24+
25+
<!-- Author tags are optional, multiple are allowed, one per tag -->
26+
<!-- Authors do not have to be maintainers, but could be -->
27+
<!-- Example: -->
28+
<!-- <author email="[email protected]">Jane Doe</author> -->
29+
30+
31+
<!-- The *depend tags are used to specify dependencies -->
32+
<!-- Dependencies can be catkin packages or system dependencies -->
33+
<!-- Examples: -->
34+
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35+
<!-- <depend>roscpp</depend> -->
36+
<!-- Note that this is equivalent to the following: -->
37+
<!-- <build_depend>roscpp</build_depend> -->
38+
<!-- <exec_depend>roscpp</exec_depend> -->
39+
<!-- Use build_depend for packages you need at compile time: -->
40+
<!-- <build_depend>message_generation</build_depend> -->
41+
<!-- Use build_export_depend for packages you need in order to build against this package: -->
42+
<!-- <build_export_depend>message_generation</build_export_depend> -->
43+
<!-- Use buildtool_depend for build tool packages: -->
44+
<!-- <buildtool_depend>catkin</buildtool_depend> -->
45+
<!-- Use exec_depend for packages you need at runtime: -->
46+
<!-- <exec_depend>message_runtime</exec_depend> -->
47+
<!-- Use test_depend for packages you need only for testing: -->
48+
<!-- <test_depend>gtest</test_depend> -->
49+
<!-- Use doc_depend for packages you need only for building documentation: -->
50+
<!-- <doc_depend>doxygen</doc_depend> -->
51+
<buildtool_depend>ament_cmake</buildtool_depend>
52+
53+
<depend>rclcpp</depend>
54+
<depend>std_msgs</depend>
55+
<depend>sensor_msgs</depend>
56+
57+
<test_depend>ament_lint_auto</test_depend>
58+
<test_depend>ament_lint_common</test_depend>
59+
60+
<!-- The export tag contains other, unspecified, tags -->
61+
<export>
62+
<!-- Other tools can request additional information be placed here -->
63+
<build_type>ament_cmake</build_type>
64+
</export>
65+
</package>

0 commit comments

Comments
 (0)