ROS2 (#65)

ROS2 example implementation

Author: benvonh <[email protected]>

CMakeLists.txt

@@ -15,33 +15,40 @@ set (STATE_MACHINE_NAME "ExoTestMachine")
 #set (STATE_MACHINE_NAME "M2DemoMachine")
 #set (STATE_MACHINE_NAME "M3DemoMachine")
 #set (STATE_MACHINE_NAME "X2DemoMachine")
+#set (STATE_MACHINE_NAME "X2ROS2DemoMachine")
 #set (STATE_MACHINE_NAME "LoggingDevice")
 
-#Use this if your state machine code folder is not in CORC 'src/apps/' folder.
-#Can be a relative or absolute path.
+# Use this if your state machine code folder is not in CORC 'src/apps/' folder.
+# Can be a relative or absolute path.
 #set (STATE_MACHINE_PATH "../")
 
 # Comment to use actual hardware, uncomment for a nor robot (virtual) app
 set(NO_ROBOT ON)
 
-# ROS Flag. set ON if you want to use ROS. Else, set OFF.
-set(USE_ROS OFF)
-# Select desired logging level (Options: TRACE, DEBUG, INFO, WARN, ERROR, CRITICAL OFF)
+# ROS Flag: set to 0 for a no ROS stateMachine, 1 for ROS 1 (use catkin build) and 2 for ROS2 (use colcon build)
+# Remember to rename select appropriate package.xml too
+set(ROS 0)
+
+# Select desired logging level (Options: TRACE, DEBUG, INFO, WARN, ERROR, CRITICAL, OFF)
 # INFO is the recommended level in normal operation
 set(CORC_LOGGING_LEVEL INFO)
 
 ################################################################################
+
+
+
 #Default path if not set
 if(NOT STATE_MACHINE_PATH)
     set (STATE_MACHINE_PATH "src/apps")
 endif()
 
 #ROS internal flags
-if(USE_ROS)
-    add_definitions(-DUSEROS)
+if(ROS GREATER 0)
+    add_definitions(-DROS=${ROS})
 endif()
 
-if(NO_ROBOT AND USE_ROS)
+#SIM flag is used for ROS1 simulation
+if(NO_ROBOT AND (ROS EQUAL 1))
     set(SIM ON)
     add_definitions(-DSIM)
 endif()
@@ -59,7 +66,7 @@ add_definitions(-DFP_STARTTPDO=0x3E1)
 ## Compile as C++14
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
-if(USE_ROS)
+if(ROS GREATER 0)
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-int-in-bool-context" )
 else()
     if(CMAKE_CROSSCOMPILING)
@@ -109,16 +116,15 @@ list (APPEND INCLUDE_DIRS lib/spdlog/include/)
 
 add_subdirectory(lib/yaml-cpp/)
 
-
 ## Hack for Yaml files path (absolute path required for ROS use, see X2Robot::initializeRobotParams)
 if(CMAKE_CROSSCOMPILING)
     add_definitions(-DBASE_DIRECTORY=.)
 else()
     add_definitions(-DBASE_DIRECTORY=${CMAKE_SOURCE_DIR})
 endif()
 
+if(ROS EQUAL 1)
 ## Add ROS 1 dependencies
-if(USE_ROS)
     #ROS 1 local compile: use catkin
     message("--catkin--")
     # Required ROS packages
@@ -153,8 +159,8 @@ if(USE_ROS)
     )
 
     generate_messages(
-       DEPENDENCIES
-       std_msgs
+    DEPENDENCIES
+    std_msgs
     )
 
     catkin_package(
@@ -175,9 +181,20 @@ if(USE_ROS)
     #include CATKIN
     include_directories(${catkin_INCLUDE_DIRS})
     set(ROS_LIBRARIES ${catkin_LIBRARIES})
+
+## Add ROS2 dependencies
+elseif(ROS EQUAL 2)
+    #ROS 2 local compile: use colcon
+    message("--colcon--")
+
+    find_package(ament_cmake REQUIRED)
+    find_package(rclcpp REQUIRED)
+    find_package(std_msgs REQUIRED)
+    find_package(sensor_msgs REQUIRED)
 endif()
 
 
+
 ## Executable name: {STATEMACHINENAME}_APP
 set (APP_NAME ${STATE_MACHINE_NAME}_APP)
 if(NO_ROBOT)
@@ -188,7 +205,7 @@ add_executable(${APP_NAME}
                 )
 
 ## Includes
-target_include_directories(${APP_NAME} PRIVATE ${INCLUDE_DIRS})
+target_include_directories(${APP_NAME} PUBLIC ${INCLUDE_DIRS})
 
 ## Set required external packages
 find_package(Threads REQUIRED)
@@ -199,12 +216,25 @@ target_link_libraries(${APP_NAME}
         yaml-cpp)
 
 ## Link ROS libraries
-if(USE_ROS)
+if(ROS EQUAL 1)
     target_link_libraries(${APP_NAME} ${ROS_LIBRARIES})
 
     # make sure configure headers are built before any node using them
     add_dependencies(${APP_NAME} ${PROJECT_NAME}_gencfg)
+
+elseif(ROS EQUAL 2)
+    include_directories(${INCLUDE_DIRS})
+
+    ament_target_dependencies(${APP_NAME} rclcpp std_msgs sensor_msgs)
+    ament_export_dependencies(rclcpp std_msgs sensor_msgs)
+
+    install(TARGETS ${APP_NAME} DESTINATION lib/${PROJECT_NAME})
+    install(PROGRAMS script/initCAN0.sh DESTINATION lib/${PROJECT_NAME})
+    install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+    install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+
+    ament_package()
 endif()
 
 
-message("-----------------------------------------------\nBuilding application ${APP_NAME}\n-----------------------------------------------")
+message("-----------------------------------------------\nBuilding application ${APP_NAME}\n-----------------------------------------------")

README.md

@@ -61,6 +61,7 @@ The following individuals have made contributions to CORC:
 - Yue Wen
 - Tim Haswell
 - Xinliang Guo
+- Benjamin von Snarski
 
 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.
 

doc/1.GettingStarted/AdvancedSimulationAndHardwareTesting.md

@@ -60,6 +60,8 @@ If you would like to do a simulation:
 Set your state machine (X2DemoMachine):
 ```set (STATE_MACHINE_NAME "X2DemoMachine")```
 
+Rename `Package.ros1.xml` to `Package.xml` in the CORC root folder.
+
 Build CORC:
 ```bash
 $ cd ~/catkin_ws

doc/1.GettingStarted/GettingStarted.md

@@ -90,9 +90,10 @@ This example enables simple functionalities of ArmMotus M3/EMU System. It shows
 1. Users looking to develop for the ArmMotus M3
 2. Users looking to develop systems with a separate user interface
 
-### [M3DemoMachine - Instructions](GSM3DemoMachine.md) 
+### [M3DemoMachine - Instructions](GSM3DemoMachine.md)
 See also the code example in `src/apps/M3DemoMachine`.
 
+
 ## Hardware Testing - AnkleMotus M1
 This example enables simple movements with the AnkleMotus M1 System.
 
@@ -104,5 +105,20 @@ This example enables simple movements with the AnkleMotus M1 System.
 1. Users looking to develop for the AnkleMotus M1
 
 ### [M1DemoMachine - Instructions](GSM1DemoMachine.md)
-See the code example in `src/apps/M1DemoMachine`.
+See also the code example in `src/apps/M1DemoMachine`.
+
+
+## ROS2 CORC application - Exoskeleton
+This example is a an example of the use of a ROS2 CORC state machine.
+It leverages the ExoMotus X2 Exoskeleton from Fourier Intelligence, and creates a ROS2 CORC package.
+
+### Requirements
+1. A development and deployment machine running Linux Ubuntu and ROS2 (Humble).
+
+### Suggested for
+1. Users looking to develop a ROS2 CORC application
+
+### [ROS2 Application - Instructions](ROS2Application.md)
+See also the code example in `src/apps/X2ROS2DemoMachine`.
+
 

doc/1.GettingStarted/ROS2Application.md

@@ -0,0 +1,67 @@
+# ROS2 Application - Instructions
+
+Minimal instructions to compile and run a CORC application as a ROS2 node able to publish the robot state.
+
+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.
+Testing the application also assumes you have an ExoMotus X2 Exoskeleton.
+
+
+> Note : these instructions are for ROS2. See [here](AdvancedSimulationAndHardwareTesting.md) for a ROS1 example.
+
+## Compilation
+
+
+```bash
+$ source /opt/ros/humble/setup.bash
+```
+
+In CORC root directory, rename ros2 package configuration file
+```bash
+$ mv package.ros2.xml package.xml
+```
+
+
+Edit the CMAKEFileList.txt to set ROS2 option and to select a ROS2 state machine:
+
+```
+...
+...
+#set (STATE_MACHINE_NAME "ExoTestMachine")
+#set (STATE_MACHINE_NAME "M1DemoMachine")
+#set (STATE_MACHINE_NAME "M1DemoMachineROS")
+#set (STATE_MACHINE_NAME "M2DemoMachine")
+#set (STATE_MACHINE_NAME "M3DemoMachine")
+#set (STATE_MACHINE_NAME "X2DemoMachine")
+set (STATE_MACHINE_NAME "X2ROS2DemoMachine")
+#set (STATE_MACHINE_NAME "LoggingDevice")
+
+# Use this if your state machine code folder is not in CORC 'src/apps/' folder.
+# Can be a relative or absolute path.
+#set (STATE_MACHINE_PATH "../")
+
+# Comment to use actual hardware, uncomment for a nor robot (virtual) app
+set(NO_ROBOT OFF)
+
+# ROS Flag: set to 0 for a no ROS stateMachine, 1 for ROS 1 (use catkin build) and 2 for ROS2 (use colcon build)
+# Remember to rename select appropriate package.xml too
+set(ROS 2)
+...
+...
+```
+
+
+From the **CANOpenController parent folder**, compile the package:
+```bash
+$ colcon build
+$ source install/setup.bash
+```
+
+
+## Run the node 
+```bash
+$ ros2 run CORC X2ROS2DemoMachine_APP
+```
+
+Here CORC is the package name and the CORC app name is the executable name.
+
+See the content of the `src/apps/X2ROS2DemoMachine` folder to see how to interact with ROS2 from within CORC.

launch/x2_ros2.launch.py

@@ -0,0 +1,56 @@
+import os
+
+from xacro import process_file
+from ament_index_python import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+	# Launch description
+	ld = LaunchDescription()
+
+	# Package share path
+	corc_path = get_package_share_directory("CORC")
+	x2_description_path = get_package_share_directory("x2_description")
+
+	# Package share paths
+	exo_path = os.path.join(corc_path, "config", "x2_params.yaml")
+	rviz_path = os.path.join(x2_description_path, "rviz", "x2.rviz")
+
+	# Process XACRO
+	urdf_xacro = process_file(
+		os.path.join(x2_description_path, "urdf", "x2_fixed_base.urdf.xacro")
+	)
+
+	# Nodes
+	exo_node = Node(
+		package="CORC",
+		executable="X2ROS2DemoMachine_APP",
+		arguments=["-can", "can0"],
+		name="x2",
+		output="screen",
+        parameters=[
+			{ "exo_path": exo_path }
+		]
+	)
+	robot_state_node = Node(
+		package="robot_state_publisher",
+		executable="robot_state_publisher",
+		name="robot_state_publisher",
+		parameters=[
+			{ "robot_description": urdf_xacro.toprettyxml(indent='	') }
+		]
+	)
+	rviz_node = Node(
+		package="rviz2",
+		executable="rviz2",
+		arguments=["-d", rviz_path],
+		name="rviz2"
+	)
+
+	# Launch description actions
+	ld.add_action(exo_node)
+	ld.add_action(robot_state_node)
+	ld.add_action(rviz_node)
+	return ld

package.ros2.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>CORC</name>
+  <version>1.0.0</version>
+  <description>The CORC package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">Benjamin von Snarski</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>Apache 2</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/x2</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>