From a7979c09e9f8c5c647d36c9eb3de219d801c300d Mon Sep 17 00:00:00 2001 From: Vincent Crocher <16175577+vcrocher@users.noreply.github.com> Date: Thu, 26 Jan 2023 15:00:00 +1100 Subject: [PATCH] ROS2 (#65) ROS2 example implementation Author: benvonh --- CMakeLists.txt | 62 ++++++++++++----- README.md | 1 + .../AdvancedSimulationAndHardwareTesting.md | 2 + doc/1.GettingStarted/GettingStarted.md | 20 +++++- doc/1.GettingStarted/ROS2Application.md | 67 +++++++++++++++++++ launch/x2_ros2.launch.py | 56 ++++++++++++++++ package.xml => package.ros1.xml | 0 package.ros2.xml | 65 ++++++++++++++++++ .../X2ROS2DemoMachine/X2ROS2DemoMachine.cpp | 62 +++++++++++++++++ .../X2ROS2DemoMachine/X2ROS2DemoMachine.h | 34 ++++++++++ src/apps/X2ROS2DemoMachine/X2ROS2Node.cpp | 67 +++++++++++++++++++ src/apps/X2ROS2DemoMachine/X2ROS2Node.h | 37 ++++++++++ src/apps/X2ROS2DemoMachine/X2ROS2State.cpp | 23 +++++++ src/apps/X2ROS2DemoMachine/X2ROS2State.h | 31 +++++++++ src/core/application.cpp | 2 +- 15 files changed, 510 insertions(+), 19 deletions(-) create mode 100644 doc/1.GettingStarted/ROS2Application.md create mode 100644 launch/x2_ros2.launch.py rename package.xml => package.ros1.xml (100%) create mode 100644 package.ros2.xml create mode 100644 src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.cpp create mode 100644 src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.h create mode 100644 src/apps/X2ROS2DemoMachine/X2ROS2Node.cpp create mode 100644 src/apps/X2ROS2DemoMachine/X2ROS2Node.h create mode 100644 src/apps/X2ROS2DemoMachine/X2ROS2State.cpp create mode 100644 src/apps/X2ROS2DemoMachine/X2ROS2State.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 0a11f5598..c2a36fef6 100644 --- a/CMakeLists.txt +++ b/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,7 +116,6 @@ 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=.) @@ -117,8 +123,8 @@ 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-----------------------------------------------") \ No newline at end of file +message("-----------------------------------------------\nBuilding application ${APP_NAME}\n-----------------------------------------------") diff --git a/README.md b/README.md index 6d6ba28fc..53ac75d20 100644 --- a/README.md +++ b/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. diff --git a/doc/1.GettingStarted/AdvancedSimulationAndHardwareTesting.md b/doc/1.GettingStarted/AdvancedSimulationAndHardwareTesting.md index 9ac3f4166..6cc0731c9 100644 --- a/doc/1.GettingStarted/AdvancedSimulationAndHardwareTesting.md +++ b/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 diff --git a/doc/1.GettingStarted/GettingStarted.md b/doc/1.GettingStarted/GettingStarted.md index c279fd122..bfed9f072 100644 --- a/doc/1.GettingStarted/GettingStarted.md +++ b/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`. + diff --git a/doc/1.GettingStarted/ROS2Application.md b/doc/1.GettingStarted/ROS2Application.md new file mode 100644 index 000000000..decbe895b --- /dev/null +++ b/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. \ No newline at end of file diff --git a/launch/x2_ros2.launch.py b/launch/x2_ros2.launch.py new file mode 100644 index 000000000..be93f5728 --- /dev/null +++ b/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 diff --git a/package.xml b/package.ros1.xml similarity index 100% rename from package.xml rename to package.ros1.xml diff --git a/package.ros2.xml b/package.ros2.xml new file mode 100644 index 000000000..df5abd866 --- /dev/null +++ b/package.ros2.xml @@ -0,0 +1,65 @@ + + + CORC + 1.0.0 + The CORC package + + + + + Benjamin von Snarski + + + + + + Apache 2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ament_cmake + + rclcpp + std_msgs + sensor_msgs + + ament_lint_auto + ament_lint_common + + + + + ament_cmake + + \ No newline at end of file diff --git a/src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.cpp b/src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.cpp new file mode 100644 index 000000000..4389afea2 --- /dev/null +++ b/src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.cpp @@ -0,0 +1,62 @@ +#include "X2ROS2DemoMachine.h" + +X2ROS2DemoMachine::X2ROS2DemoMachine(int argc, char **argv) : StateMachine() +{ + // Create and set X2Robot object + setRobot(std::make_unique()); + + // Configure ROS2 initialisation options and disable SIGINT capture (handled by CORC) + rclcpp::InitOptions ros_init = rclcpp::InitOptions(); + ros_init.shutdown_on_signal = false; + rclcpp::init(argc, argv, ros_init); + + // Create the ROS2 node and pass a reference to the X2 Robot object + m_Node = std::make_shared("x2", get_robot()); + + // Add some state that also holds reference to the ROS2 node + addState("state", std::make_shared(get_robot(), get_node())); + + // Set the initial state + setInitState("state"); +} + +void +X2ROS2DemoMachine::end() +{ + spdlog::info("State machine end"); +} + +void +X2ROS2DemoMachine::init() +{ + get_robot()->initialiseNetwork(); +} + +void +X2ROS2DemoMachine::hwStateUpdate() +{ + // Update robot parameters + get_robot()->updateRobot(); + // Call custom publish method for publishing joint states of X2 Robot + get_node()->publish_joint_states(); + // Allow for the ROS2 node to execute callbacks (e.g., subscriptions) + rclcpp::spin_some(get_node()->get_interface()); +} + +bool +X2ROS2DemoMachine::configureMasterPDOs() +{ + return get_robot()->configureMasterPDOs(); +} + +X2Robot * +X2ROS2DemoMachine::get_robot() +{ + return static_cast(_robot.get()); +} + +const std::shared_ptr & +X2ROS2DemoMachine::get_node() +{ + return m_Node; +} diff --git a/src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.h b/src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.h new file mode 100644 index 000000000..596d39d91 --- /dev/null +++ b/src/apps/X2ROS2DemoMachine/X2ROS2DemoMachine.h @@ -0,0 +1,34 @@ +/** + * \file X2ROS2DemoMachine.h + * \author Benjamin von Snarski + * \version 0.1 + * \date 2022-10-24 + * \copyright Copyright (c) 2022 + * \brief An example CORC state machine implementing the ROS2 client library. + */ +#ifndef SRC_X2ROS2DEMOMACHINE_H +#define SRC_X2ROS2DEMOMACHINE_H + +#include "X2Robot.h" +#include "X2ROS2Node.h" +#include "X2ROS2State.h" +#include "StateMachine.h" + +class X2ROS2DemoMachine : public StateMachine +{ +public: + X2ROS2DemoMachine(int argc, char **argv); + + void end() override; + void init() override; + void hwStateUpdate() override; + bool configureMasterPDOs() override; + + X2Robot * get_robot(); + const std::shared_ptr & get_node(); + +private: + std::shared_ptr m_Node; +}; + +#endif//SRC_X2ROS2DEMOMACHINE_H \ No newline at end of file diff --git a/src/apps/X2ROS2DemoMachine/X2ROS2Node.cpp b/src/apps/X2ROS2DemoMachine/X2ROS2Node.cpp new file mode 100644 index 000000000..8d4637822 --- /dev/null +++ b/src/apps/X2ROS2DemoMachine/X2ROS2Node.cpp @@ -0,0 +1,67 @@ +#include "X2ROS2Node.h" + +X2ROS2Node::X2ROS2Node(const std::string &name, X2Robot *robot) + : Node(name), m_Robot(robot) +{ + // Create an example subscription + m_Sub = create_subscription( + "example_topic", 10, std::bind(&X2ROS2Node::string_callback, this, _1) + ); + // Create a joint state publisher + m_Pub = create_publisher( + "joint_states", 10 + ); +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr +X2ROS2Node::get_interface() +{ + // Must have this method to return base interface for spinning + return this->get_node_base_interface(); +} + +void +X2ROS2Node::string_callback(const std_msgs::msg::String::SharedPtr msg) +{ + // Example callback for subscription + spdlog::info("Received ({}) in string callback", msg->data); +} + +void +X2ROS2Node::publish_joint_states() +{ + // Instantiate joint state message + sensor_msgs::msg::JointState msg; + + // Assign current header time stamp + msg.header.stamp = this->now(); + + // Use this naming scheme for robot state publisher to recognise + msg.name = { + "left_hip_joint", + "left_knee_joint", + "right_hip_joint", + "right_knee_joint", + "world_to_backpack" + }; + + // Copy position, velocity and toroque from X2 Robot + msg.position.assign( + m_Robot->getPosition().data(), + m_Robot->getPosition().data() + m_Robot->getPosition().size() + ); + msg.velocity.assign( + m_Robot->getVelocity().data(), + m_Robot->getVelocity().data() + m_Robot->getVelocity().size() + ); + msg.effort.assign( + m_Robot->getTorque().data(), + m_Robot->getTorque().data() + m_Robot->getTorque().size() + ); + + // Add backpack angle to positions for visualisation (RViz) + msg.position.push_back(m_Robot->getBackPackAngleOnMedianPlane() - M_PI_2); + + // Publish the joint state message + m_Pub->publish(msg); +} \ No newline at end of file diff --git a/src/apps/X2ROS2DemoMachine/X2ROS2Node.h b/src/apps/X2ROS2DemoMachine/X2ROS2Node.h new file mode 100644 index 000000000..3aa09b33e --- /dev/null +++ b/src/apps/X2ROS2DemoMachine/X2ROS2Node.h @@ -0,0 +1,37 @@ +/** + * \file X2ROS2Node.h + * \author Benjamin von Snarski + * \version 0.1 + * \date 2022-10-24 + * \copyright Copyright (c) 2022 + * \brief An example ROS2 node that also holds a reference to the robot object. + */ +#ifndef SRC_X2ROS2NODE_H +#define SRC_X2ROS2NODE_H + +#include "X2Robot.h" + +#include +#include +#include + +using std::placeholders::_1; + +class X2ROS2Node : public rclcpp::Node +{ +public: + X2ROS2Node(const std::string &name, X2Robot *robot); + + void string_callback(const std_msgs::msg::String::SharedPtr msg); + void publish_joint_states(); + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_interface(); + +private: + X2Robot *m_Robot; + + rclcpp::Subscription::SharedPtr m_Sub; + rclcpp::Publisher::SharedPtr m_Pub; +}; + +#endif//SRC_X2ROS2NODE_H \ No newline at end of file diff --git a/src/apps/X2ROS2DemoMachine/X2ROS2State.cpp b/src/apps/X2ROS2DemoMachine/X2ROS2State.cpp new file mode 100644 index 000000000..51bc6efe9 --- /dev/null +++ b/src/apps/X2ROS2DemoMachine/X2ROS2State.cpp @@ -0,0 +1,23 @@ +#include "X2ROS2State.h" + +X2ROS2State::X2ROS2State(X2Robot *robot, const std::shared_ptr node) + : m_Robot(robot), m_Node(node) +{ +} + +void +X2ROS2State::exit() +{ + spdlog::info("Exited X2ROS2State"); +} + +void +X2ROS2State::entry() +{ + spdlog::info("Entered X2ROS2State"); +} + +void +X2ROS2State::during() +{ +} \ No newline at end of file diff --git a/src/apps/X2ROS2DemoMachine/X2ROS2State.h b/src/apps/X2ROS2DemoMachine/X2ROS2State.h new file mode 100644 index 000000000..216701fd2 --- /dev/null +++ b/src/apps/X2ROS2DemoMachine/X2ROS2State.h @@ -0,0 +1,31 @@ +/** + * \file X2ROS2State.h + * \author Benjamin von Snarski + * \version 0.1 + * \date 2022-10-24 + * \copyright Copyright (c) 2022 + * \brief An example state that publishes the current X2 robot joint states and subscribes to an example topic. + * Also holds reference to the ROS2 node. + */ +#ifndef SRC_X2ROS2STATE_H +#define SRC_X2ROS2STATE_H + +#include "State.h" +#include "X2Robot.h" +#include "X2ROS2Node.h" + +class X2ROS2State : public State +{ +public: + X2ROS2State(X2Robot *robot, const std::shared_ptr node); + + void exit() override; + void entry() override; + void during() override; + +private: + X2Robot *m_Robot; + const std::shared_ptr m_Node; +}; + +#endif//SRC_X2ROS2STATE_H \ No newline at end of file diff --git a/src/core/application.cpp b/src/core/application.cpp index ebc7bc7a9..7d4e32250 100644 --- a/src/core/application.cpp +++ b/src/core/application.cpp @@ -19,7 +19,7 @@ std::unique_ptr stateMachine; /******************** RUNS BEFORE CO_init() ********************/ void app_communicationReset(int argc, char *argv[]) { -#ifdef USEROS +#if ROS > 0 stateMachine = std::make_unique(argc, argv); #else stateMachine = std::make_unique();