Motion Communication Example

ROS1

Usage

Open a new terminal and run the following command to check the communication status:

sudo systemctl status transfer

transfer-active

transfer-inactive

If the status shows active, the communication function can be used directly;
If the status shows inactive, run the following command to start the communication:

sudo systemctl start transfer

To stop the communication:

sudo systemctl stop transfer

To view communication logs:

journalctl -fu transfer

Open a new terminal and use ROS rostopic commands to check robot status:

rostopic info xxxxxx
rostopic echo xxxxxx     # xxxxxx refers to specific topic names; subscribe to topics in your code for secondary development

Use the /cmd_vel topic to send velocity commands to the motion host. The topic message type geometry_msgs/Twist is defined as:

geometry_msgs/Vector3 linear   # Linear velocity (m/s)
  float64 x                    # Forward velocity, positive forward
  float64 y                    # Lateral velocity, positive left
  float64 z                    # Invalid parameter
geometry_msgs/Vector3 angular  # Angular velocity (rad/s)
  float64 x                    # Invalid parameter
  float64 y                    # Invalid parameter
  float64 z                    # Turning angular velocity, positive left

a) Users can publish this topic in C++ and Python programs compiled with ROS. You can also open a terminal for debugging:

rostopic pub /cmd_vel geometry_msgs/Twist

b) Don’t run yet. Add a space after the statement, press Tab to auto-fill the message type content:

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
  angular:
  x: 0.0
  y: 0.0
  z: 0.0
"

c) Use arrow keys to move the cursor and modify velocity values, then add -r 10 after geometry_msgs/Twist to set the publish frequency (10 times per second):

rostopic pub /cmd_vel geometry_msgs/Twist -r 10 "linear:
  x: 0.2
  y: 0.1
  z: 0.0
  angular:
  x: 0.0
  y: 0.0
  z: 0.3
"

d) Run the command to publish the topic.

e) The transfer program subscribes to this topic and converts it to UDP commands for the motion host.

f) After the transfer program is running, stand up the robot and enable autonomous mode in the APP settings page.

Note: Please debug in an open area to avoid injury to people or damage. In emergencies, press the software emergency stop or disable autonomous mode.

Program Structure

/home/ysc/lite_cog/transfer/src
├── CMakeLists.txt
└── message_transformer
    ├── CMakeLists.txt
    ├── include
    │   ├── protocol.h
    │   └── sensor_logger.h
    ├── launch
    │   └── message_transformer.launch
    ├── msg
    │   ├── SimpleCMD.msg
    │   └── ComplexCMD.msg
    ├── package.xml
    └── src
        ├── nx2app.cpp
        ├── qnx2ros.cpp
        ├── ros2qnx.cpp
        └── sensor_checker.cpp

nx2app.cpp handles UDP communication between the perception host and the gamepad APP. The command struct:

class SimpleCMD{
public:
    int32_t cmd_code;
    int32_t cmd_value;
    int32_t type;
};

qnx2ros.cpp receives data from the motion host and converts it to ROS topics:

leg_odom_pub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("leg_odom", 1);
leg_odom_pub2_ = nh.advertise<nav_msgs::Odometry>("leg_odom2", 1);
joint_state_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
imu_pub_ = nh.advertise<sensor_msgs::Imu>("/imu/data", 1);
handle_pub_ = nh.advertise<geometry_msgs::Twist>("/handle_state", 1);
ultrasound_pub_ = nh.advertise<std_msgs::Float64>("/us_publisher/ultrasound_distance", 1);

ros2qnx.cpp subscribes to topics and converts them to UDP datagrams for the motion host:

ros::Subscriber vel_sub = nh.subscribe("cmd_vel", 1, &ROS2QNX::CmdVelCallback, &ros2qnx);
ros::Subscriber vel_sub2 = nh.subscribe("cmd_vel_corrected", 1, &ROS2QNX::CmdVelCallback, &ros2qnx);
ros::Subscriber simplecmd_sub = nh.subscribe("simple_cmd", 1, &ROS2QNX::SimpleCMDCallback, &ros2qnx);
ros::Subscriber complexcmd_sub = nh.subscribe("complex_cmd", 1, &ROS2QNX::ComplexCMDCallback, &ros2qnx);

ROS2

Usage

Check the communication status:

sudo systemctl status transfer_ros2

transfer-ros2-status

If active, communication can be used directly;
If inactive, start it:

sudo systemctl start transfer_ros2

To stop:

sudo systemctl stop transfer_ros2

To view logs:

journalctl -fu transfer_ros2

Use ROS2 topic commands:

ros2 topic info xxxxxx
ros2 topic echo xxxxxx

Use /cmd_vel topic. Message type geometry_msgs/msg/Twist:

geometry_msgs/msg/Vector3 linear   # Linear velocity (m/s)
  float64 x                        # Forward velocity, positive forward
  float64 y                        # Lateral velocity, positive left
  float64 z                        # Invalid parameter
geometry_msgs/msg/Vector3 angular  # Angular velocity (rad/s)
  float64 x                        # Invalid parameter
  float64 y                        # Invalid parameter
  float64 z                        # Turning angular velocity, positive left

a) Publish via command:

ros2 topic pub /cmd_vel -r 10 geometry_msgs/msg/Twist "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

b) Run the command to publish.

c) The transfer program subscribes and converts to UDP.

d) Stand up the robot and enable autonomous mode.

Note: Debug in an open area. In emergencies, press software emergency stop or disable autonomous mode.

Program Structure

ysc@lite:~/lite_cog_ros2/transfer/src$ tree .
├── transfer
│   ├── CMakeLists.txt
│   ├── include
│   │   └── protocol.hpp
│   ├── launch
│   │   └── transfer_launch.py
│   ├── package.xml
│   └── src
│       ├── Jetson2App.cpp
│       ├── Jetson2Motion.cpp
│       ├── SensorChecker.cpp
│       └── SensorsLogger.hpp
└── transfer_interfaces
    ├── CMakeLists.txt
    ├── msg
    │   ├── MotionComplexCMD.msg
    │   └── MotionSimpleCMD.msg
    └── package.xml

Jetson2App.cpp handles UDP communication between perception host and gamepad APP:

class SimpleCMD{
public:
    int32_t cmd_code;
    int32_t cmd_value;
    int32_t type;
};

Jetson2Motion.cpp contains MotionReceiver and MotionSender. MotionReceiver publishes:

leg_odom_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("leg_odom", 10);
leg_odom_pub2_ = create_publisher<nav_msgs::msg::Odometry>("leg_odom2", 10);
joint_state_pub_ = create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
imu_pub_ = create_publisher<sensor_msgs::msg::Imu>("/imu/data", 10);
handle_pub_ = create_publisher<geometry_msgs::msg::Twist>("/handle_state", 10);
ultrasound_pub_ = create_publisher<std_msgs::msg::Float64>("/us_publisher/ultrasound_distance", 10);

MotionSender subscribes:

cmd_vel_sub_ = create_subscription<geometry_msgs::msg::Twist>(
    "cmd_vel", 10,
    std::bind(&MotionSender::CmdVelCallback, this, std::placeholders::_1));
cmd_vel_corrected_sub_ = create_subscription<geometry_msgs::msg::Twist>(
    "cmd_vel_corrected", 10,
    std::bind(&MotionSender::CmdVelCallback, this, std::placeholders::_1));
simplecmd_sub = create_subscription<transfer_interfaces::msg::MotionSimpleCMD>(
    "simple_cmd", 10,
    std::bind(&MotionSender::SimpleCMDCallback, this, std::placeholders::_1));
complexcmd_sub = create_subscription<transfer_interfaces::msg::MotionComplexCMD>(
    "complex_cmd", 10,
    std::bind(&MotionSender::ComplexCMDCallback, this, std::placeholders::_1));