[1 fix]更新docker-compose自启动。

bash/autobuild.sh

@@ -1,7 +0,0 @@
-#! /bin/bash
-source /opt/ros/galactic/setup.bash
-pushd ..
-mkdir -p install/saturn_controller_interface/lib
-cp sdk/lib/x86_64/libmc_client.so install/saturn_controller_interface/lib/
-colcon build
-popd

docker-compose.yml

@@ -2,12 +2,12 @@ version: '3.3'
 
 services:
     base:
-      image: im.daystar.cloud:10443/daystar_bot/base_saturn:v1.8.1_beta_build_20240621
+      image: im.daystar.cloud:10443/daystar_bot/base_saturn:v1.8.1_beta_build_20240802
       container_name: base
       privileged: true
       tty: true
       stdin_open: true
-      network_mode: "host"  # 设置网络模式为 host
+      network_mode: "host"
       ipc: host
       environment:
        - DISPLAY
@@ -18,4 +18,4 @@ services:
        - /dev:/dev
        - /config/dds_router/cyclonedds.xml:/root/cyclonedds.xml
        - ./src:/root/saturn_ros2/src
-      command: bash
+      command: bash -c "source /opt/ros/galactic/setup.bash && source /root/saturn_ros2/install/setup.bash && ros2 launch saturn_controller saturn_ros2.launch.py"

docker/Dockerfile

@@ -38,10 +38,8 @@ RUN apt-get update && apt-get install -y \
 # copy ros2 pkgs to ws
 WORKDIR $ROS2_WS
 COPY src src
-COPY sdk sdk
-COPY bash bash
 RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
-    cd bash && ./autobuild.sh && cd ..\
+    colcon build && \
     rm -rf log build src
 
 RUN echo "source /root/saturn_ros2/install/setup.bash " >> /root/.bashrc

docker/docker_build.sh

@@ -19,5 +19,3 @@ cd $ROOT_PATH
 docker build -t $TAG -f $BUILD_PATH/Dockerfile  .
 
 docker image push $TAG
-
-

sdk/include/com_define.h

@@ -1,80 +0,0 @@
-#ifndef _COM_CONFIG_H_
-#define _COM_CONFIG_H_
-
-#define THE_TRQ_SENSOR_NUM 18
-#define PI 3.14159265358979323846
-#define MAX_SLAVE_NUM 24
-#define CONTROL_RATE 1000
-
-#define THE_CAN_DRIVER_NUM 18
-
-//single leg test
-// #define MOTOR_NUM 3
-// #define LEG_NUMBER 1
-
-//six legs
-#define MOTOR_NUM 18
-#define LEG_NUMBER 6
-
-//视觉落脚点
-#define HEIGHT_MAP_HEIGHT 80 //改成80
-#define HEIGHT_MAP_WIDTH 80 //改成80
-
-#define GRID_SIZE 0.04
-
-#define DEPTH_IMAGE_HEIGHT 300
-#define DEPTH_IMAGE_WIDTH 400
-
-#define RGB_IMAGE_HEIGHT 300
-#define RGB_IMAGE_WIDTH 400
-
-#define DRIVER_WATCH_DOG_TIMEOUT 3000
-
-typedef enum _DRIVE_MODE{
-    POSITION_MODE = 0,
-    VELOCITY_MODE,
-    TORQUE_MODE,
-    GOHOME_MODE,
-    TORQUE_FF_MODE,
-    PP_MODE
-}DRIVE_MODE;
-
-typedef enum _DRIVER_PARAM_SETTING_CMD{
-    CMD_NULL = 0,
-    CLEAR_BISS_ERR,
-    READ_BISS_ERR_INFO,
-    READ_DRIVER_STRING_ERR,
-    READ_DRIVER_COMMON_ERR,
-    SET_ENCODER_POWER_50,
-    SET_ENCODER_POWER_26,
-    SET_ENCODER_POWER_14,
-    SET_ENCODER_POWER,
-    READ_DRIVER_STRING_STO_ERR,
-    CMD_MAX
-}DRIVER_PARAM_SETTING_CMD;
-
-typedef enum _EN_DRIVER_ECAT_SLAVES_TYPE{
-	NONE_DRIVER_TYPE = 0,
-	LING_KONG_DRIVER_TYPE = 1,
-	LENOVO_S_CAN_DRIVER_TYPE = 2,
-	LENOVO_M_DRIVER_TYPE = 3,
-	MOTOREVO_DRIVER_TYPE = 4,
-	SYNAPTICON_DRIVER_TYPE = 5,
-}EN_DRIVER_ECAT_SLAVES_TYPE;
-
-typedef enum _EN_TRQ_SENSOR_ECAT_SLAVES_TYPE{
-	NONE_TRQ_SENSOR_TYPE = 0,
-	SJTU_TRQ_SENSOR_TYPE = 1,
-	YULI_TRQ_SENSOR_TYPE = 2,
-}EN_TRQ_SENSOR_ECAT_SLAVES_TYPE;
-
-typedef enum _ROBORT_BODY_MODEL{
-	GS_SYNAPTICON = 0,
-	IS_FORCE_POS_MIXING = 1,
-	IS_ONLY_FORCE = 2,
-}ROBORT_BODY_MODEL;
-
-#define UNUSED(x) (void)(x)
-#define LOG_LENGTH 256
-
-#endif

sdk/include/common.h

@@ -1,612 +0,0 @@
-#ifndef COMMON_H
-#define COMMON_H
-
-#include <cstring>
-#include <string>
-#include "com_define.h"
-// #include <Eigen/Dense>
-
-typedef struct ROBOT_CONFIG_
-{
-    //utility
-    double dt;
-    double legIsRightLst[LEG_NUMBER];
-
-    //kinematics and dynamics
-    double l0;
-    double l1;
-    double l2;
-    double lMin2;
-    double hip[LEG_NUMBER][3];
-    double legs_offset[LEG_NUMBER];
-
-    //dynamics
-    double m;
-    double inertia[3][3];
-    double leg_m;
-
-    //control paramters
-    double legStiffSw[3];
-    double legStiffSt[3];
-    double legDamp[3];
-    double legStiffSphSw[3];
-    double legStiffSphSt[3];
-    double legDampSph[3];
-    double jntStiffSw[3];
-    double jntStiffSt[3];
-    double jntDamp[3];
-    double qddKp[3];
-    double qddKd[3];
-
-    //gait parameters
-    double offset_lst[LEG_NUMBER];
-    double stdP[LEG_NUMBER][3];
-    double bodyH;
-
-    // constraints
-    double wdMax;
-
-    //compensation
-    double fricComp[3];
-    
-}ROBOT_CONFIG;
-
-typedef union {
-    float array[3];
-    struct {
-        float x;
-        float y;
-        float z;
-    } axis;
-} MeasurementVector;
-
-typedef union {
-    float array[3];
-    struct {
-        float roll;
-        float pitch;
-        float yaw;
-    } axis;
-} EulerAngleVector;
-
-/**
- * @brief Quaternion.
- */
-typedef union {
-    float array[4];
-    struct {
-        float w;
-        float x;
-        float y;
-        float z;
-    } element;
-} QuaternionVector;
-
-typedef union {
-    float array[3];
-    struct {
-        float x;
-        float y;
-        float z;
-    } axis;
-} GPSVector;
-
-typedef struct TOP_CONFIG_INFO_{
-    std::string robot_type;
-    ROBORT_BODY_MODEL body_model;
-}TOP_CONFIG_INFO;
-
-namespace robot_control
-{
-    namespace common
-    {
-        const int GAIT_MSG_CAPACITY = 200;
-
-        enum class CoreState {
-            BOOT = 0,
-            IDLE = 1,
-            JOINTS_TEST = 2,
-            STAND_STILL = 3,
-            IN_PLANNING = 4
-        };
-
-        enum GaitMsgType //这里如果修改成enum class，需要芳勇配合修改参数接口
-        {
-            GMT_NO_MSG = 0,
-            GMT_START = 1,
-            GMT_ONLINE = 2,
-            GMT_EM_STOP = 3,
-            GMT_JOINT_CONTROL = 4,
-            GMT_GAIT_MODE = 5
-        };
-
-        enum GaitID //这里如果修改成enum class，需要芳勇配合修改参数接口
-        {
-            GID_NO_GAIT    = 0,
-            GID_IDLE_GAIT  = 1,
-            GID_STANDSTILL = 2,
-            GID_RECOVER    = 3,
-            GID_NIMBLE     = 4,
-            GID_JOINT_TEST = 5,
-            GID_FOLD_LEG_TROT  = 6,//六足机器人上，中间两条腿抬起的步态
-            GID_JOINT_TRAJECTORY_CONTROL  = 7,
-            GID_POSE_ADJUSTMENT=8,
-            GID_FOOT_ADJUST=9,
-        };
-
-        class GaitMsgHeader
-        {
-        public:
-            short int gait_msg_type;
-            short int gait_id;
-            int data_length;
-        };
-
-        class GaitMsg
-        {
-        public:
-            GaitMsgHeader header;
-            char data[GAIT_MSG_CAPACITY];
-            inline int getAllDataLength()
-            {
-                return header.data_length + sizeof(GaitMsgHeader);
-            }
-        };
-
-        class Pose
-        {
-        public:
-            float q[4];
-            float gyro[3];
-            float acc[3];
-        };
-
-        class ModelInput
-        {
-        public:
-            unsigned long count;
-            double actual_pos[MOTOR_NUM];
-            double actual_vel[MOTOR_NUM];
-            double actual_trq[MOTOR_NUM]; // trq read from the motor current
-            double mea_trq[MOTOR_NUM];    // trq read from the torque sensor
-            Pose body_pose;
-            GaitMsg gait_msg;
-            bool is_stand_up;
-            int planner_stage;
-        public:
-            ModelInput()
-            {
-                is_stand_up = false;
-            }
-        };
-
-        struct JointTestData
-        {
-            unsigned int axis_bitmask;
-            float mag;
-            float period;
-            float kp_pos;
-            float kd_pos;
-            float kp_cur;
-            float mu_clb;
-        };
-
-        class ModelOutput
-        {
-        public:
-            unsigned long count;
-            double target_pos[MOTOR_NUM];
-            double target_vel[MOTOR_NUM];
-            double target_trq[MOTOR_NUM];
-            double target_kp[MOTOR_NUM];
-            double target_kd[MOTOR_NUM];
-            CoreState core_state;
-            GaitID current_gait_id;
-        };
-
-        typedef struct MC_QUATERNIONS_{
-            float x;
-            float y;
-            float z;
-            float w;
-        }MC_QUATERNIONS;
-
-        typedef struct MC_COORDINATE_{
-            float x;
-            float y;
-            float z;
-        }MC_COORDINATE;
-
-        typedef struct MC_MOTION_IMU_DATA_{
-            uint64_t timestamp;
-            MC_QUATERNIONS orientation;
-            double orientation_covariance[9];
-            MC_COORDINATE angular_velocity;
-            double angular_velocity_covariance[9];
-            MC_COORDINATE linear_acceleration;
-            double linear_acceleration_covariance[9];
-        }MC_MOTION_IMU_DATA;
-
-        typedef struct JOY_KEY_TYPE_{
-            bool stand_up;
-            int move_mode;
-            int speed_gear;
-            double dynamic_speed_x;
-            double dynamic_speed_y;
-            double static_speed_x;
-            double static_speed_y;
-        }JOY_KEY_TYPE;
-
-        typedef enum ROBOT_DIRECTION_{
-            FRONT = 0,
-            REAR,
-            LEFT,
-            RIGHT
-        }ROBOT_DIRECTION;
-
-        typedef struct ROBOT_TWIST_{
-            MC_COORDINATE linear;
-            MC_COORDINATE angular;
-        }ROBOT_TWIST;
-
-        //0: Trot（默认）1: 奔跑  2: 跳跃 3: Saturn标志动作
-        typedef enum ROBOT_GAIT_{
-            TROT = 0,
-            RUN,
-            JUMP,
-            SATURN,
-            NIMBLE
-        }ROBOT_GAIT;
-
-        //int32 model = 1; //0: 自主导航， 1,遥控控制
-        typedef enum ROBOT_CONTROL_MOVE_MODE_{
-            NAVIGATION = 0,
-            JOY
-        }ROBOT_CONTROL_MOVE_MODE;
-
-        typedef enum DRIVER_ENABLE_STATE_{
-            UNKNOW = 0,
-            ENABLED,
-            ENABLING,
-            ENABLE_FAILED,
-            DISABLED
-        }DRIVER_ENABLE_STATE;
-
-        //scene = 1;	//0: 平地 , 1: 楼梯 , 2: 沟壑 , 3: 陡坡 , 4: 限高
-        typedef enum SCENE_TYPE_{
-			NULL_SCENE = 0,
-            LIE_DOWN = 1,
-            WALKING = 2,
-            STAIRS = 3,
-            CHARGE = 4,
-            PERCEIVE_STAIRS = 5
-        }SCENE_TYPE;
-
-        //机器人场景切换状态枚举
-        typedef enum SCENE_SWITCH_STATE_{
-			NULL_SCENE_SWITCH = 0,
-            SWITCHING = 1,
-            SWITCH_SUCCEEDED = 2,
-            SWITCH_FAILED = 3
-        }SCENE_SWITCH_STATE;
-
-        typedef enum BODY_POSTURE_STATE_{
-            NULL_POSTURE = 0,
-            STARTMOVING = 1,
-            MOVING = 2,
-			LIEDOWN = 3,
-            STANDUP = 4,
-            CHARGING=5  //在桩上  可能是充电，也可能已经充满了
-        }BODY_POSTURE_STATE;
-
-        //相机位置  
-        typedef enum CHARGE_POSITION_STATE_{
-            NULL_POSITION = 0,
-            POSITION_OFF = 1,    //找不到位置 默认状态
-            POSITION_FINDING = 2,//搜寻位置此时需要用户手动控制机器人进行移动
-            POSITION_ONLINE = 3, //位置信息在线
-            POSITION_FAILED = 4  //用户调整超时后进入该状态，考虑二维码破损造成的情况
-        }CHARGE_POSITION_STATE;
-        //上桩过程枚举
-        typedef enum CHARGE_SWITCH_STATE_{
-            NULL_CHARGE_SWITCH = 0,
-            EXIT_SUCCEEDED = 1,  
-            EXITING_TILE = 2, 
-            EXIT_FAILED=3, 
-            ENTER_SUCCEEDED = 4,
-            ENTERING_TILE = 5,
-            ENTER_FAILED=6   //尝试上桩超过三次后进入该状态
-        }CHARGE_SWITCH_STATE;
-
-        typedef enum AUTO_CHARGE_STATE_{
-            null_charge = 0,
-            uncharge = 1,
-            charging = 2,
-            charge_finished = 3
-        }AUTO_CHARGE_STATE;
-
-        typedef struct ROBOT_BATTERY_INFO_{
-            float level; //机器人电池当前电量
-            AUTO_CHARGE_STATE state; //用来表示机器人电池 没充电 ，充电中，充电结束的状态 
-        }ROBOT_BATTERY_INFO;
-
-        typedef struct BODY_ODOMETRY_{
-            ROBOT_TWIST twist;
-            //geometry_msgs/Pose Point & Quaternion
-            MC_COORDINATE point;
-            MC_QUATERNIONS quaternion;
-            // 6*6 covariance matrix
-            // repeated pose = 2;
-        }BODY_ODOMETRY;
-
-        //充电相关状态 
-        typedef struct ROBOT_CHARGE_STATUS_{ //运动控制主机和手机通讯的内容
-            std::string charge_tile_name;           //充电桩唯一标识符 （考虑当前是否需要）
-            CHARGE_POSITION_STATE position_state;   //检测到二维码位置时进行进行自动充电模式 反馈到手机
-            CHARGE_SWITCH_STATE   charge_switch_state; //上桩过程状态   反馈到手机
-            bool charge_succeeded;                  //电池与充电桩连接成功正在充电标志位  该标志位需要BMS与充电桩通信确认
-            ROBOT_BATTERY_INFO battery_info;
-            bool user_quit_charge_flag;
-        }ROBOT_CHARGE_STATUS;
-
-        typedef struct JETSON_CHARGE_STATE_{ //回冲功能上传Jetson的信息
-            long heartbeat;                //心跳信息
-            bool sensnor;                  //是否打开相机    Jetson 读该信息 
-        }JETSON_CHARGE_STATE;
-
-        typedef struct JETSON_CHARGE_COMMAND_{ //Jetson主机下发指令到运动主机的信息
-            bool tag_state;				     //检测到二维码    Jetson 写该信息
-            MC_QUATERNIONS position;         //充电桩位置信息  Jetson 写该位置
-        }JETSON_CHARGE_COMMAND;
-        
-        //Jetson回冲相关的状态统一放在这里
-        typedef struct JETSON_CHARGE_INFO_{ //统一管理接口
-            JETSON_CHARGE_STATE charge_state;       //与充电相关的机器人状态
-            JETSON_CHARGE_COMMAND charge_command;    //充电相关的位置信息
-        }JETSON_CHARGE_INFO;
-
-        //视觉落脚点功能，从运动控制主机上传到Jetson主机所用的数据类型,实机
-        typedef struct JETSON_VISUAL_FOOTHOLD_STATE_{
-            uint64_t timestamp;
-            robot_control::common::BODY_ODOMETRY robot_odometry;
-            float touch_detection[LEG_NUMBER];
-            float tip_position_wrt_hip[LEG_NUMBER * 3];
-            float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3]; //规划出的足端相对机器人base的位置
-            bool camera_switch; //是否打开相机
-        }JETSON_VISUAL_FOOTHOLD_STATE;
-
-        //视觉落脚点功能，从Jetson主机下发数据到运动控制主机的数据类型
-        typedef struct JETSON_VISUAL_FOOTHOLD_COMMAND_{
-            float height_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
-            uint64_t timestamp;
-            float map_center_point[2];
-            float body_pos_act[3];
-            float body_vel_act[3]; //现在机器人身体实际的速度并没有被用到，临时借用一下作为雷达最新的数据
-            float body_pos_est[3];
-            float body_vel_est[3];
-            float body_orn_act[4];
-            
-            float body_gyr_act[3];
-            float body_orn_est[4];
-            float body_gyr_est[3];
-            float neigh_height_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
-            float neigh_confidence_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
-            float neigh_roughness_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH];
-        }JETSON_VISUAL_FOOTHOLD_COMMAND;
-
-        typedef struct DEPTH_IMAGE_{
-            int image[DEPTH_IMAGE_HEIGHT][DEPTH_IMAGE_WIDTH];
-        }DEPTH_IMAGE;
-
-        typedef struct RGB_IMAGE_{
-            int image[RGB_IMAGE_HEIGHT][RGB_IMAGE_WIDTH];
-        }RGB_IMAGE;
-
-        //视觉落脚点相关的状态统一放在这里
-        typedef struct JETSON_VISUAL_FOOTHOLD_INFO_{ //统一管理接口
-            uint64_t timestamp;
-            JETSON_VISUAL_FOOTHOLD_COMMAND vis_foothold_command;  
-            float touch_detection[LEG_NUMBER];
-            float tip_position_wrt_hip[LEG_NUMBER * 3]; //当前机器人足端相对髋关节坐标系的位置
-            float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3]; //规划出的足端相对机器人base的位置
-            bool camera_switch; //是否打开相机
-        }JETSON_VISUAL_FOOTHOLD_INFO;
-
-        typedef struct ROBOT_DRIVER_VALUE_{
-            double position;
-            double velocity;
-            double torque;
-            double torque_sensor;
-        }ROBOT_DRIVER_VALUE;
-
-        typedef enum NAV_OR_JOY_MODE_{
-            nall_control = 0,
-            nav_control = 1,
-            joy_control = 2
-        }NAV_OR_JOY_MODE;
-
-        typedef enum LIE_DOWN_ADJUST_{
-            null = 0,
-            adjusting = 1,
-            nimble_stop = 2,
-            finish = 3
-        }LIE_DOWN_ADJUST_;
-
-        //停障功能从navigation端接收到的结果
-        //在目前的使用中，当float<=-2，代表存在障碍物，否则代表不存在障碍物
-        typedef struct GUARDIAN_VELOCITY_DECAY_RATIO_{
-            float left_ratio;
-            float front_ratio;
-            float right_ratio;
-            float rear_ratio;
-        }GUARDIAN_VELOCITY_DECAY_RATIO;
-
-        typedef enum CALIBRATE_POSE_STATE_{
-            NULL_STATE = 0,
-            START_CALIBRATE = 1,
-            CALIBRATE_SUCCESS = 2,
-            CALIBRATE_FAILED = 3
-        }CALIBRATE_POSE_STATE;
-
-        typedef struct ROBOT_COMMON_STATUS_{
-            int network; //机器人当前网络状态
-            BODY_POSTURE_STATE belie_or_stand; //机器人处于站立趴下状态(目前不用，芳勇认为测试中可能会用)
-            bool emergency; //是否是处于紧急状态
-            bool avoidance; //自动避障状态
-            long int heartbeat; //心跳
-            float cur_speed; //当前速度
-            float cur_height; //当前高度
-            MC_QUATERNIONS position; //机器人世界坐标系下的位置，xyz与yaw角
-            float max_height; //机器人最大高度
-            float max_speed; //机器人最大速度
-            SCENE_SWITCH_STATE cur_scene_switch_state; //切换场景是否成功
-            SCENE_TYPE cur_scene; //当前处于的场景,预设站立 预设应该是趴下比较合理
-            DRIVER_ENABLE_STATE driver_enable_state;
-            //机器人里程计信息
-            BODY_ODOMETRY odometry;
-            float tip_position_wrt_hip[LEG_NUMBER*3];//足端相对髋关节坐标系位置
-            float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3];
-            float touch_detection[LEG_NUMBER];//触地检测，范围从0到1.0, 等于1.0为绝对触地，后期会变为概率，但目前还是只有1或者0两种可能
-
-            //错误代码
-            int error_code;
-            ROBOT_CHARGE_STATUS charge_status;
-            bool salute_state;
-            NAV_OR_JOY_MODE joy_mode;
-
-            bool guardian_switch; //表示当前停障功能的开关的状态
-            bool visual_foothold_switch; //表示当前视觉落脚点功能开关的状态
-            GUARDIAN_VELOCITY_DECAY_RATIO guardian_velocity_decay_ratio; //军营用于显示前左右三个方位的障碍 (芳哥，这是新加接口)
-
-            LIE_DOWN_ADJUST_ lie_down_adjust; //用于判断趴下前是否正在做足端位置调整  
-            float speed_bar_on_controller; //机器人遥控器上当前选中的线速度
-            CALIBRATE_POSE_STATE calibrate_pos_state;
-        }ROBOT_COMMON_STATUS;
-
-        typedef enum JOINT_LIMIT_STATE_{
-            NULL_LIMIT = 0,
-            LIMITED = 1,
-            NOLIMITED = 2
-        }JOINT_LIMIT_STATE;
-        //18个关节的关节状态
-        typedef struct ROBOT_JOINT_STATUS_{
-            std::string name;
-            float current;//电机数据（电流值）
-            float joint_temperature;//关节温度
-            float torque_sensor_data;//扭矩传感器数据
-            double position;
-            double velocity;
-            double effort;
-            double max_limit;
-            double min_limit;
-            JOINT_LIMIT_STATE limit_state;
-            unsigned short driver_error_code;
-            double target_position;
-            double target_velocity;
-            double target_effort;
-        }ROBOT_JOINT_STATUS;
-
-        typedef struct ROBOT_JOINTS_STATUS_{
-            ROBOT_JOINT_STATUS joints[MOTOR_NUM];
-            long int heartbeat;
-        }ROBOT_JOINTS_STATUS;
-
-        typedef struct ROBOT_PER_CYCLE_STATUS_{
-            int network;//机器人当前网络状态
-            bool lie_stand_state; //joystick连接状态
-            bool emergency; //是否是处于紧急状态
-            bool avoidance; //自动避障状态
-            long int heartbeat; //心跳
-            float speed;
-            MC_QUATERNIONS position;
-        }ROBOT_PER_CYCLE_STATUS;
-
-        typedef struct ROBOT_TARGET_VALUE_{
-            ROBOT_DRIVER_VALUE driver[MOTOR_NUM];
-            long int heartbeat;
-        }ROBOT_TARGET_VALUE;
-
-        typedef enum enFormOfMovement_
-        {
-            JOINTS_CONTROL = 0,
-            MOTION_CONTROL = 1
-        }enFormOfMovement;
-
-        typedef struct DRIVER_CONFIG_PARAMETER_{
-            std::string motor_name;
-            double position_scale;
-            double velocity_scale;
-            double torque_scale;
-            bool motor_direction;
-            bool direction_correction;
-            int index_map;
-            bool be_choosed;
-            double init_pos_offset;
-            double init_pos_offset_rough;
-            double position_min_limit;
-            double position_min_safety_limit;
-            double position_max_limit;
-            double position_max_safety_limit;
-            double min_limit_kp;
-            double min_limit_ki;
-            double min_limit_kd;
-            double max_limit_kp;
-            double max_limit_ki;
-            double max_limit_kd;
-            int convert_to_deceleration_end;
-            int one_turn_encorder_value;
-        }DRIVER_CONFIG_PARAMETER;
-
-        typedef struct SENSOR_CONFIG_PARAMETER_{
-            double trq_sensor_scale;
-            double trq_sensor_offset;
-            bool trq_sensor_direction;
-            int index_map;
-        }SENSOR_CONFIG_PARAMETER;
-
-        typedef struct _JOINTS_CONFIG_INFO{
-            robot_control::common::DRIVER_CONFIG_PARAMETER driver_data[MOTOR_NUM];
-            robot_control::common::SENSOR_CONFIG_PARAMETER sensor_data[THE_TRQ_SENSOR_NUM];
-            bool trq_have_been_calibrated;
-        }JOINTS_CONFIG_INFO;
-
-        typedef struct _ROBOT_SPECIAL_CONFIG_INFO{
-            std::string bms_address;
-            std::string light_address;
-            EN_DRIVER_ECAT_SLAVES_TYPE driver_type;
-            EN_TRQ_SENSOR_ECAT_SLAVES_TYPE trq_sensor_type;
-        }ROBOT_SPECIAL_CONFIG_INFO;
-
-        typedef enum _ROBOT_BODY_TYPE{
-            ROBOT_BODY_NON = 0,
-            ROBOT_BODY_REAL,
-            ROBOT_BODY_WEBOTS,
-            ROBOT_BODY_GAZEBO
-        }ROBOT_BODY_TYPE;
-
-        typedef enum LOG_LEVEL_{
-            LOG_LEVEL_NON = 0,
-            LOG_LEVEL_DEBUG,
-            LOG_LEVEL_NOTE,
-            LOG_LEVEL_WARNING,
-            LOG_LEVEL_ERROR,
-            LOG_LEVEL_ALL,
-            LOG_LEVEL_MAX
-        }LOG_LEVEL;
-
-        typedef enum LOG_MODULE_NAME_{
-            MODULE_NON = 0,
-            MODULE_CONTROL_CORE,
-            MODULE_ENTRY,
-            LOG_MODULE_MAX
-        }LOG_MODULE_NAME;
-
-        typedef struct ROBOT_POSE_ESTIMATE_{
-            MC_COORDINATE point;
-            MC_QUATERNIONS quaternion;
-        }ROBOT_POSE_ESTIMATE;
-        //Log config
-    } // namespace common
-}
-
-#endif

sdk/include/mc_client_interface.h

@@ -1,122 +0,0 @@
-#ifndef MC_CLIENT_INTERFACE_H
-#define MC_CLIENT_INTERFACE_H
-
-#include <iostream>
-#include <memory>
-#include <string>
-#include <vector>
-#include "common.h"
-
-// extern "C" {
-
-using namespace robot_control;
-
-bool rcClientInterfaceInit(std::string target_ip);
-std::string rcClientInterfaceGetMcVersion(const int& index);
-std::string rcClientInterfaceGetRobotName(const int& index);
-bool rcClientInterfaceGetMcMoveStatus(int index, common::MC_QUATERNIONS *state_attribute);
-
-// imu 数据上报
-bool rcClientInterfaceGetImuData(common::MC_MOTION_IMU_DATA *imu_data);
-bool rcClientInterfaceGetImuStreamData(std::vector<common::MC_MOTION_IMU_DATA> *imu_data_list);
-
-// 坐下（默认）/起立
-bool rcClientInterfaceLieDownOrStandUp(robot_control::common::NAV_OR_JOY_MODE from,bool liedown_standup);
-// 使能
-bool rcClientInterfaceDriverEnable(robot_control::common::NAV_OR_JOY_MODE from, bool driver_enable);
-// 前进方向(前后左右)
-bool rcClientInterfaceDirectionMovement(robot_control::common::NAV_OR_JOY_MODE from,
-            common::ROBOT_TWIST rc_direct, int64_t time_millis);
-// 步态
-bool rcClientInterfaceSetGait(robot_control::common::NAV_OR_JOY_MODE from, common::ROBOT_GAIT gain);
-// 高度调节条
-bool rcClientInterfaceBodyHighAdjust(robot_control::common::NAV_OR_JOY_MODE from, int scale);
-// 速度调节条
-bool rcClientInterfaceSpeedAdjust(robot_control::common::NAV_OR_JOY_MODE from, int scale);
-// 场景设定
-bool rcClientInterfaceSetScene(robot_control::common::NAV_OR_JOY_MODE from, common::SCENE_TYPE scene);
-// 机器人一键自恢复
-bool rcClientInterfaceSelfRecovery(robot_control::common::NAV_OR_JOY_MODE from);
-// 力矩传感器清零
-bool rcClientInterfaceZeroTorqueSensor(robot_control::common::NAV_OR_JOY_MODE from);
-// 机器人常规状态信息
-bool rcClientInterfaceGetCommonStatus(common::ROBOT_COMMON_STATUS *com_state);
-// 机器人本体状态信息
-bool rcClientInterfaceGetJointsStatus(common::ROBOT_JOINTS_STATUS *state);
-// 机器人日志信息
-bool rcClientInterfaceLogUpdate(std::vector<std::string> &log_list);
-bool rcClientInterfaceSetJoyKeyValues(common::JOY_KEY_TYPE joy_key_attribute);
-// 遥控或手柄控制模式
-bool rcClientInterfaceDriverEstop(robot_control::common::NAV_OR_JOY_MODE from);
-// 遥控或手柄控制模式
-bool rcClientInterfaceResumeEstop(robot_control::common::NAV_OR_JOY_MODE from);
-// 软件急停
-bool rcClientInterfaceDriverSoftEstop(robot_control::common::NAV_OR_JOY_MODE from);
-// 恢复软件急停
-bool rcClientInterfacercResumeSoftEstop(robot_control::common::NAV_OR_JOY_MODE from);
-// 自动充电
-bool rcClientInterfaceSetAutoCharge(
-            robot_control::common::NAV_OR_JOY_MODE from,
-            common::AUTO_CHARGE_STATE charge_state);
-// 设置导航控制或者遥控控制
-bool rcClientInterfaceSetNavOrJoyControl(
-            common::NAV_OR_JOY_MODE nav_joy);
-// 设置机器人位姿
-bool rcClientInterfaceSetPoseEstimate(
-            robot_control::common::NAV_OR_JOY_MODE from,
-            common::ROBOT_POSE_ESTIMATE estimate);
-// 机器人充电识别二维码
-bool rcClientInterfaceIdentifyQRCode(robot_control::common::NAV_OR_JOY_MODE from);
-bool rcClientInterfaceEnterOrExitCharge(
-            robot_control::common::NAV_OR_JOY_MODE from,
-            bool driver_enable);
-bool rcClientInterfaceAutoRechargeCmd(
-            robot_control::common::NAV_OR_JOY_MODE from,
-            common::JETSON_CHARGE_COMMAND charge_command);
-bool rcClientInterfaceGetChargeState(common::JETSON_CHARGE_STATE *charge_state);
-bool rcClientInterfaceLightStripControl(
-            robot_control::common::NAV_OR_JOY_MODE from,
-            std::string port, int cmd, int deviceAddr, int data);
-bool rcClientInterfaceSetGuardian(
-			common::NAV_OR_JOY_MODE from,
-			float velocity_decay_ratio,
-			std::string trigger_source);
-bool rcClientInterfaceSetGuardianSwitch(
-            robot_control::common::NAV_OR_JOY_MODE from,
-            bool enable);
-bool rcClientInterfaceSetVisualFootholdSwitch(
-            robot_control::common::NAV_OR_JOY_MODE from,
-            bool enable);
-bool rcClientInterfaceSetVisualFootholdCommand(
-			common::JETSON_VISUAL_FOOTHOLD_COMMAND map);
-bool rcClientInterfaceGetVisualFootholdState(
-			common::JETSON_VISUAL_FOOTHOLD_STATE *foothold_state);
-bool rcClientInterfaceGetDepthImage(
-			common::DEPTH_IMAGE *depth_image);
-bool rcClientInterfaceGetRgbImage(
-			common::RGB_IMAGE *rgb_image);
-bool rcClientInterfaceGetMotionControllerTimestamp(unsigned long *timestamp);
-bool rcClientInterfaceDemoControl(
-			common::NAV_OR_JOY_MODE from,
-			int mode,
-			bool enable);
-
-bool rcClientInterfaceSetJointsMode(int mode);
-bool rcClientInterfaceSetJointsPVQ(common::ROBOT_TARGET_VALUE value);
-bool rcClientInterfaceGetJointsPVQ(common::ROBOT_TARGET_VALUE *value);
-bool rcClientInterfaceDriverParameterSetting(
-            robot_control::common::NAV_OR_JOY_MODE from,
-            int motor_index,
-            DRIVER_PARAM_SETTING_CMD cmd);
-bool rcClientInterfaceSetFormOfMovement(int mode);
-bool rcClientInterfaceSelectSomeJoints(unsigned int mask);
-bool rcClientInterfaceSelectSingleJoint(int axis_index, bool be_selected);
-bool rcClientInterfaceClearDriverError(unsigned int mask);
-bool rcClientInterfaceSetMaxTorqueLimit(int axis_index, double max_torque);
-bool rcClientInterfaceReCalibrateAllDriverAndSave(bool is_rough);
-bool rcClientInterfaceSetJointTestParameter (common::JointTestData joint_test_data);
-bool rcClientInterfaceSineMoveJointTestSwitch (bool be_enable);
-bool rcClientInterfaceQuitJointTest ();
-
-// }
-#endif // MC_CLIENT_INTERFACE_H

src/saturn_controller/.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "functional": "cpp"
+    }
+}

src/saturn_controller/CMakeLists.txt

@@ -26,17 +26,16 @@ include_directories(./include)
 
 #message(COMMAND echo "The current binary directory is: ${CMAKE_CURRENT_BINARY_DIR}")
 file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/../../install/saturn_controller/lib)
-file(COPY ./sdk/lib/x86_64/libmc_client.so DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../../install/saturn_controller/lib/)
+file(COPY ./sdk/lib/x86_64/libmc_client.so DESTINATION 
+  ${CMAKE_CURRENT_BINARY_DIR}/../../install/saturn_controller/lib/)
 
 link_directories(${CMAKE_CURRENT_BINARY_DIR}/../../install/saturn_controller/lib) 
 
 file(GLOB main_files src/*.cpp)
 
 add_library(saturn_controller SHARED src/saturn_controller.cpp ${main_files})
-
 target_link_libraries(saturn_controller yaml-cpp -lmc_client)
 ament_target_dependencies(saturn_controller ${THIS_PACKAGE_INCLUDE_DEPENDS})
-
 rclcpp_components_register_nodes(saturn_controller "saturn_ros2::SaturnController")
 
 install(TARGETS saturn_controller
@@ -45,7 +44,7 @@ install(TARGETS saturn_controller
   RUNTIME DESTINATION bin
 )
 
-install(DIRECTORY config
+install(DIRECTORY config launch
   DESTINATION share/${PROJECT_NAME}/
 )
 

src/saturn_controller/README.md

@@ -1,2 +1,4 @@
 source /opt/ros/galactic/setup.bash && ros2 run rclcpp_components component_container
-source /opt/ros/galactic/setup.bash && ros2 component load /ComponentManager saturn_controller saturn_ros2::SaturnController
+source /opt/ros/galactic/setup.bash && ros2 component load /ComponentManager saturn_controller saturn_ros2::SaturnController
+
+source /opt/ros/galactic/setup.bash && ros2 component load /component_container saturn_controller saturn_ros2::SaturnController

src/saturn_controller/config/real/config.yaml

@@ -2,4 +2,4 @@ config:
   motion_computer_ip_address: 192.168.100.103
   main_timer_frequency: 50
   imu_timer_frequency: 300
-  legged_odom_timer_frequency: 100
+  legged_odom_timer_frequency: 100

src/saturn_controller/config/simulation/config.yaml

@@ -1,5 +1,5 @@
 config:
-  motion_computer_ip_address: 127.0.0.1
+  motion_computer_ip_address: 192.168.100.103
   main_timer_frequency: 50
   imu_timer_frequency: 300
-  legged_odom_timer_frequency: 100
+  legged_odom_timer_frequency: 100

src/saturn_controller/launch/saturn_ros2.launch.py

@@ -0,0 +1,28 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+import launch
+import launch_ros.actions
+
+def generate_launch_description():
+    component_manager_node = launch_ros.actions.Node(
+        package='rclcpp_components',
+        executable='component_container',
+        name='component_container',
+        output='screen'
+    )
+
+    composable_node = launch_ros.actions.LoadComposableNodes(
+        target_container='/component_container',
+        composable_node_descriptions=[
+            launch_ros.descriptions.ComposableNode(
+                package='saturn_controller',
+                plugin='saturn_ros2::SaturnController',
+                name='saturn_controller'
+            ),
+        ]
+    )
+
+    return launch.LaunchDescription([
+        component_manager_node,
+        composable_node
+    ])

src/saturn_controller_interface/.gitignore

@@ -1,53 +0,0 @@
-~$
-.swp$
-build/
-bin/
-lib/
-msg_gen/
-srv_gen/
-msg/.*Action\.msg$
-msg/.*ActionFeedback\.msg$
-msg/.*ActionGoal\.msg$
-msg/.*ActionResult\.msg$
-msg/.*Feedback\.msg$
-msg/.*Goal\.msg$
-msg/.*Result\.msg$
-msg/_.*\.py$
-
-\.pcd$
-.pyc$
-__pycache__$
-
-# Generated by dynamic reconfigure
-\.cfgc$
-/cfg/cpp/
-/cfg/.*\.py$
-
-# Ignore generated docs
-.dox$
-.wikidoc$
-
-# eclipse stuff
-.project
-.cproject
-
-# qcreator stuff
-CMakeLists.txt.user
-
-srv/_.*\.py$
-\.pcd$
-.pyc$
-qtcreator-*
-*.user
-
-/planning/cfg
-/planning/docs
-/planning/src
-
-*~$
-
-# Emacs
-.#*
-
-# Catkin custom files
-CATKIN_IGNORE

src/saturn_controller_interface/.travis.yml

@@ -1,25 +0,0 @@
-sudo: required
-dist: trusty
-language: generic
-compiler:
-  - gcc
-install:
-  - export CI_ROS_DISTRO=indigo
-  - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
-  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-  - sudo apt-get update
-  - sudo apt-get install python-rosdep -y
-  - sudo `which rosdep` init
-  - rosdep update
-  - rosdep install --default-yes --from-paths ./ --rosdistro $CI_ROS_DISTRO
-script:
-  - source /opt/ros/$CI_ROS_DISTRO/setup.bash
-  - mkdir ../build && cd ../build
-  - cmake $TRAVIS_BUILD_DIR -DCMAKE_INSTALL_PREFIX=../install
-  - make -j1
-  - make -j1 tests
-  - make -j1 run_tests
-  - catkin_test_results .
-  - make -j1 install
-notifications:
-  email: false

src/saturn_controller_interface/CHANGELOG.rst

@@ -1,10 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package teleop_twist_joy
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-
-0.1.0 (2014-07-25)
-------------------
-* Added configurations for Logitech Attack3 and Extreme 3D Pro joysticks.
-* Initial version, with example config for PS3 joystick.
-* Contributors: Mike Purvis, Tony Baltovski

src/saturn_controller_interface/CMakeLists.txt

@@ -1,92 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(saturn_controller_interface)
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-find_package(ament_cmake REQUIRED)
-
-find_package(geometry_msgs REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_components REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(saturn_msgs REQUIRED)
-find_package(daystar_navigation_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(ysc_robot_msgs REQUIRED)
-
-include_directories(include
-    ${PROJECT_SOURCE_DIR}/../../sdk/include/
-    ${PROJECT_SOURCE_DIR}/../../sdk/include/
-    /opt/grpc/include/)
-
-add_library(${PROJECT_NAME} SHARED src/saturn_controller_interface.cpp)
-
-include(GenerateExportHeader)
-generate_export_header(${PROJECT_NAME} EXPORT_FILE_NAME ${PROJECT_NAME}/${PROJECT_NAME}_export.h)
-target_include_directories(${PROJECT_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>")
-set_target_properties(${PROJECT_NAME} PROPERTIES EXPORT_HEADER_DIR "${CMAKE_CURRENT_BINARY_DIR}")
-
-ament_target_dependencies(${PROJECT_NAME}
-  "geometry_msgs"
-  "rclcpp"
-  "rclcpp_components"
-  "sensor_msgs"
-  "saturn_msgs"
-  "nav_msgs"
-  "std_msgs"
-  "ysc_robot_msgs"
-  "daystar_navigation_msgs"
-)
-
-rclcpp_components_register_nodes(${PROJECT_NAME}
-  "saturn_controller_interface::SaturnControllerInterface")
-
-link_directories(${PROJECT_SOURCE_DIR}/../../sdk/lib/x86_64) #添加动态连接库的路径
-#target_link_libraries(project_name -lmxnet ) #添加libmxnet.so
-
-add_executable(${PROJECT_NAME}_node src/saturn_controller_node.cpp)
-target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} -lmc_client)
-set_target_properties(${PROJECT_NAME}_node
-  PROPERTIES OUTPUT_NAME ${PROJECT_NAME}_node PREFIX "")
-
-install(TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin)
-
-install(TARGETS ${PROJECT_NAME}_node
-  DESTINATION lib/${PROJECT_NAME})
-
-install(DIRECTORY include/
-  DESTINATION include)
-
-install(DIRECTORY
-  launch
-  DESTINATION share/${PROJECT_NAME})
-
-install(DIRECTORY
-  config
-  DESTINATION share/${PROJECT_NAME})
-
-install(DIRECTORY
-  urdf
-  DESTINATION share/${PROJECT_NAME})
-
-install(FILES
-  ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}/${PROJECT_NAME}_export.h
-  DESTINATION include/${PROJECT_NAME})
-
-install(DIRECTORY launch config
-  DESTINATION share/${PROJECT_NAME})
-
-ament_export_include_directories(include)
-ament_export_libraries(${PROJECT_NAME})
-ament_package()

src/saturn_controller_interface/LICENSE.txt

@@ -1,18 +0,0 @@
-Software License Agreement (BSD)
-
-Redistribution and use in source and binary forms, with or without modification, are permitted provided that
-the following conditions are met:
- * Redistributions of source code must retain the above copyright notice, this list of conditions and the
-   following disclaimer.
- * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
-   following disclaimer in the documentation and/or other materials provided with the distribution.
- * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
-   products derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
-RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
-PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
-DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
-OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE

src/saturn_controller_interface/config/saturn_base.config.yaml

@@ -1,6 +0,0 @@
-# Saturn base configure file
-saturn/saturn_controller_interface_node:
-  ros__parameters:
-    remote_controller_ip: 192.168.1.103
-    imu_sender_frequency: 140
-    common_status_frequency: 80

src/saturn_controller_interface/include/saturn_controller_interface/saturn_controller_interface.hpp

@@ -1,205 +0,0 @@
-#ifndef SATURN_CONTROLLER_INTERFACE_H
-#define SATURN_CONTROLLER_INTERFACE_H
-
-#include <rclcpp/rclcpp.hpp>
-#include "saturn_controller_interface/saturn_controller_interface_export.h"
-#include <geometry_msgs/msg/twist.hpp>
-#include <sensor_msgs/msg/joy.hpp>
-#include <sensor_msgs/msg/imu.hpp>
-
-
-#include "mc_client_interface.h"
-#include <saturn_msgs/srv/rc_version.hpp>
-#include <saturn_msgs/srv/driver_enable.hpp>
-#include <saturn_msgs/srv/velocity_scale.hpp>
-#include <saturn_msgs/srv/height_scale.hpp>
-#include <saturn_msgs/srv/auto_charge.hpp>
-#include <saturn_msgs/srv/gait_scene.hpp>
-#include <saturn_msgs/srv/nav_or_manual.hpp>
-#include <daystar_navigation_msgs/srv/guardian.hpp>
-#include <daystar_navigation_msgs/msg/guardian.hpp>
-#include <saturn_msgs/msg/common_status.hpp>
-
-#include <nav_msgs/msg/odometry.hpp>
-#include <sensor_msgs/msg/joint_state.hpp>
-#include <sensor_msgs/msg/battery_state.hpp>
-#include <std_msgs/msg/int32.hpp>
-#include <std_msgs/msg/u_int32.hpp>
-#include <std_msgs/msg/u_int8.hpp>
-#include <std_msgs/msg/bool.hpp>
-#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <ysc_robot_msgs/srv/robot_command.hpp>
-#include <saturn_msgs/srv/guardian_switch.hpp>
-#include <std_msgs/msg/float32_multi_array.hpp>
-
-// #include "mc_server.h"
-namespace saturn_controller_interface
-{
-/**
- * Class implementing a basic Joy -> Twist translation.
- */
-class SATURN_CONTROLLER_INTERFACE_EXPORT SaturnControllerInterface : public rclcpp::Node
-{
-public:
-  explicit SaturnControllerInterface(const rclcpp::NodeOptions& options);
-  virtual ~SaturnControllerInterface();
-
-private:
-  // subscription
-  //1, 导航下发速度
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub;
-
-  //2, tag_detected (std_msgs::msg::Bool)
-  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tag_detected_sub;
-
-  //3, pose in tag (geometry_msgs::msg::PoseStamped)
-  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_in_tag_sub;
-
-  //4, task_mode (std_msgs::msg::Bool)
-  //机器人是否处于任务模式，与机器人自身状态
-  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr task_mode_sub;
-
-  // publiser
-  // 1, 六足机器人腿部里程计
-  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr leg_odom_publisher;
-  // 2, robot_state(std_msgs::msg::UInt32): 机器人基本状态
-  // 0 -> standdown
-  // 1 -> standingup
-  // 2 -> standup
-  // 3 -> enable forcecontrol
-  // 4 -> motion
-  // 5 -> sitting down
-  // 6 -> fall down
-  rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr robot_state_publisher;
-  // 3, dock_state(std_msgs::msg::UInt32): 机器人上桩状态
-  // 0 -> idle
-  // 1 -> going to dock
-  // 2 -> stand above dock
-  // 3 -> adjust pose
-  // 4 -> lie on dock
-  // 5 -> standing after charging
-  // 6 -> leaving dock
-  // 7 -> leave dock
-  rclcpp::Publisher<std_msgs::msg::UInt32>::SharedPtr dock_state_publisher;
-
-  // 4, 控制模式-手动、导航
-  // control_mode(std_msgs::msg::UInt8)：
-  // 0 -> 手持遥控
-  // １ -> 自动（导航）
-  rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr nav_or_manual_publisher;
-
-  // 5, gait(std_msgs::msg::UInt8): 机器人当前步态
-  // 0 -> 行走
-  // 1 -> 楼梯
-  // 2 -> 湿滑
-  // 3 -> 跑步
-  //saturn 设置六足机器人场景步态（1, 趴下, 2，行走步态， 3, 楼梯步态）
-  rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr gait_scene_publisher;
-
-  // 6, battery_state(sensor_msgs::msg::BatteryState)：电池电量信息,目前只有percentage字段有意义,
-  // 粗略代表机器人当前电量百分比
-  rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_state_publisher;
-
-  // 7, robot_mode 机器人模式，用于向巡检平台上报
-  // 0->空闲（默认）
-  // 1->手持遥控
-  // 2->任务
-  // 3->充电中
-  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr robot_mode_publisher;
-
-  // 8，charge_state(std_msgs::msg::Bool)机器人在充电桩充电返回true，否则返回false
-  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr charge_state_publisher;
-
-  //9, 六足机器人IMU数据
-  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher;
-
-  // test publisher a array of tip's torque
-  rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr tip_position_wrt_hip_publisher;
-  rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr touch_detection_publisher;
-  // services 
-  // 1, robot_command (ysc_robot_msgs::srv::RobotCommand);
-  // 对机器人的基本控制接口，如切换步态，上下桩，起立，趴下
-  //当前电量、最高关节温度、最高驱动温度、六足机器人步态状态（趴下、行走步态、楼梯步态）
-  rclcpp::Service<ysc_robot_msgs::srv::RobotCommand>::SharedPtr robot_command_service;
-  // subscribe
-  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr vel_msg);
-  void tagDetectedCallback(const std_msgs::msg::Bool::SharedPtr vel_msg);
-  void poseInTagCallback(const geometry_msgs::msg::PoseStamped::SharedPtr vel_msg);
-  void taskModeCallback(const std_msgs::msg::Bool::SharedPtr vel_msg);
-
-  void poseEstimateCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr robot_pose);
-
-  void guardianCallback(const daystar_navigation_msgs::msg::Guardian::SharedPtr guardian_msg);
-
-  void ImuPublisherThread();
-  void LeggedOdomPublisherThread();
-  void CommonPublisherThread();
-
-  void RobotCommandCallback(const ysc_robot_msgs::srv::RobotCommand::Request::SharedPtr request,
-                  ysc_robot_msgs::srv::RobotCommand::Response::SharedPtr response);
-  void RcVersionCallback(const saturn_msgs::srv::RcVersion::Request::SharedPtr request,
-                  saturn_msgs::srv::RcVersion::Response::SharedPtr response);
-  void DriverEnableCallback(const saturn_msgs::srv::DriverEnable::Request::SharedPtr request,
-                  saturn_msgs::srv::DriverEnable::Response::SharedPtr response);
-  void HeightScaleCallback(const saturn_msgs::srv::HeightScale::Request::SharedPtr request,
-                  saturn_msgs::srv::HeightScale::Response::SharedPtr response);
-  void VelocityScaleCallback(const saturn_msgs::srv::VelocityScale::Request::SharedPtr request,
-                  saturn_msgs::srv::VelocityScale::Response::SharedPtr response);
-  void setGaitSceneCallback(const saturn_msgs::srv::GaitScene::Request::SharedPtr request,
-                saturn_msgs::srv::GaitScene::Response::SharedPtr response);
-  void autoChargeCallback(const saturn_msgs::srv::AutoCharge::Request::SharedPtr request,
-                saturn_msgs::srv::AutoCharge::Response::SharedPtr response);
-  void navOrManualControlCallback(const saturn_msgs::srv::NavOrManual::Request::SharedPtr request,
-                saturn_msgs::srv::NavOrManual::Response::SharedPtr response);
-  void guardianSwitchCallback(const saturn_msgs::srv::GuardianSwitch::Request::SharedPtr request,
-                saturn_msgs::srv::GuardianSwitch::Response::SharedPtr response);
-  void guardianServiceCallback(const daystar_navigation_msgs::srv::Guardian::Request::SharedPtr request,
-                daystar_navigation_msgs::srv::Guardian::Response::SharedPtr response);
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_estimate_sub;
-
-  rclcpp::Publisher<saturn_msgs::msg::CommonStatus>::SharedPtr common_status_publisher;
-  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher;
-  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr error_state_publisher;
-
-  rclcpp::Service<saturn_msgs::srv::RcVersion>::SharedPtr rc_version_service;
-  rclcpp::Service<saturn_msgs::srv::DriverEnable>::SharedPtr driver_enable_service;
-  rclcpp::Service<saturn_msgs::srv::HeightScale>::SharedPtr height_scale_service;
-  rclcpp::Service<saturn_msgs::srv::VelocityScale>::SharedPtr velocity_scale_service;
-  rclcpp::Service<saturn_msgs::srv::GaitScene>::SharedPtr gait_scene_service;
-  rclcpp::Service<saturn_msgs::srv::AutoCharge>::SharedPtr auto_charge_service;
-  rclcpp::Service<saturn_msgs::srv::NavOrManual>::SharedPtr nav_or_manual_service;
-
-  rclcpp::Service<daystar_navigation_msgs::srv::Guardian>::SharedPtr guardian_service;
-  rclcpp::Subscription<daystar_navigation_msgs::msg::Guardian>::SharedPtr guardian_sub;
-  rclcpp::Service<saturn_msgs::srv::GuardianSwitch>::SharedPtr guardian_switch_service;
-
-  std::thread imu_thread_;
-  bool run_imu_thread_;
-  std::thread legged_odom_thread;
-  bool run_legged_odom_thread;
-
-  std::thread common_state_thread_;
-  bool run_common_state_thread_;
-
-  std::string remote_ip;
-  int imu_frquency;
-  int com_frquency;
-  bool stand_up;
-  int move_mode;
-  int speed_gear;
-  double dynamic_speed_x;
-  double dynamic_speed_y;
-  double static_speed_x;
-  double static_speed_y;
-  long int robot_heartbeat;
-  robot_control::common::SCENE_TYPE robot_cur_scene;
-  bool cur_robot_is_idle;
-  int robot_mode_backup;
-
-  float guardian_velocity_decay_ratio_ = 1.0;
-};
-
-}
-
-#endif

src/saturn_controller_interface/launch/saturn_controller_interface.launch.py

@@ -1,59 +0,0 @@
-import os
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import Command
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-from launch_ros.actions import Node
-from launch_ros.parameter_descriptions import ParameterValue
-from launch_ros.substitutions import FindPackageShare
-
-
-
-def generate_launch_description():
-    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
-
-
-    base_config_file=os.path.join(
-        get_package_share_directory('saturn_controller_interface'), 'config',
-        'saturn_base.config.yaml'),
-
-   
-    urdf_path = PathJoinSubstitution([FindPackageShare('saturn_controller_interface'), 'urdf', 'saturn_base_imu_rslidar_tf2.urdf.xml'])
-
-    robot_description_content = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
-
-
-    remappings = [('/tf', 'tf'),
-                  ('/tf_static', 'tf_static')]
-    namespace = os.getenv("ROBOT_NAME", '')
-
-    return LaunchDescription([
-        DeclareLaunchArgument(
-            'use_sim_time',
-            default_value='false',
-            description='Use simulation (Gazebo) clock if true'
-            ),
-        Node(
-            package='saturn_controller_interface',
-            namespace=namespace,
-            executable='saturn_controller_interface_node',
-            name='saturn_controller_interface_node',
-            parameters=[base_config_file],
-            #remappings=[
-            #    ("cmd_vel", "/cmd_vel")
-            #]
-        ),
-        Node(
-            package='robot_state_publisher',
-            namespace=namespace,
-            executable='robot_state_publisher',
-            name='robot_joint_state_publisher',
-            output='screen',
-            parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_description_content}],
-            remappings = remappings
-            ),
-    ])

src/saturn_controller_interface/package.xml

@@ -1,34 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>saturn_controller_interface</name>
-  <version>2.4.3</version>
-  <description>The interface of dog. Here using joy to control the dog's moving.</description>
-  <maintainer email="yufy4@lenovo.com">Fangyong.Yu</maintainer>
-
-  <license>BSD</license>
-
-  <url type="website">http://wiki.ros.org/saturn_controller_interface</url>
-  <author email="yufy4@lenovo.com">Fangyong.Yu</author>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>geometry_msgs</depend>
-  <depend>rclcpp</depend>
-  <depend>rclcpp_components</depend>
-  <depend>sensor_msgs</depend>
-  <depend>saturn_msgs</depend>
-  <depend>nav_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>daystar_navigation_msgs</depend>
-  <depend>ysc_robot_msgs</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-  <test_depend>launch_ros</test_depend>
-  <test_depend>launch_testing_ament_cmake</test_depend>
-  <test_depend>launch_testing_ros</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/saturn_controller_interface/src/saturn_controller_interface.cpp

@@ -1,766 +0,0 @@
-#include <rcutils/logging_macros.h>
-
-#include <cinttypes>
-#include <functional>
-
-#include <map>
-#include <memory>
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp_components/register_node_macro.hpp>
-
-#include <set>
-#include <string>
-
-#include "saturn_controller_interface/saturn_controller_interface.hpp"
-
-#define ROS_INFO_NAMED RCUTILS_LOG_INFO_NAMED
-#define ROS_INFO_COND_NAMED RCUTILS_LOG_INFO_EXPRESSION_NAMED
-
-namespace saturn_controller_interface {
-
-/**
- * Constructs SaturnControllerInterface.
- */
-SaturnControllerInterface::SaturnControllerInterface(const rclcpp::NodeOptions& options)
-    : Node("saturn_controller_interface_node", options) 
-{
-    this->declare_parameter<std::string>("remote_controller_ip", "192.168.100.103");
-    this->get_parameter("remote_controller_ip",remote_ip);
-    rcClientInterfaceInit(remote_ip);
-
-    RCLCPP_WARN(this->get_logger(), "--SaturnControllerInterface 00 --ip=[%s] ", remote_ip.c_str());
-
-    // subscription
-    //1, 导航下发速度
-    cmd_vel_sub = this->create_subscription<geometry_msgs::msg::Twist>(
-          "cmd_vel", rclcpp::QoS(10),
-          std::bind(&SaturnControllerInterface::cmdVelCallback, this,
-                    std::placeholders::_1));
-
-    //2, tag_detected (std_msgs::msg::Bool)
-    tag_detected_sub = this->create_subscription<std_msgs::msg::Bool>(
-          "tag_detected", rclcpp::QoS(10),
-          std::bind(&SaturnControllerInterface::tagDetectedCallback, this,
-                    std::placeholders::_1));
-
-    //3, pose in tag (geometry_msgs::msg::PoseStamped)
-    pose_in_tag_sub = this->create_subscription<geometry_msgs::msg::PoseStamped>(
-          "pose_in_tag", rclcpp::QoS(10),
-          std::bind(&SaturnControllerInterface::poseInTagCallback, this,
-                    std::placeholders::_1));
-
-    guardian_sub = this->create_subscription<daystar_navigation_msgs::msg::Guardian>(
-          "guardian", rclcpp::QoS(10),
-          std::bind(&SaturnControllerInterface::guardianCallback, this,
-                    std::placeholders::_1));
-
-    //4, task_mode (std_msgs::msg::Bool)
-    //机器人是否处于任务模式，与机器人自身状态 （不支持）
-    task_mode_sub = this->create_subscription<std_msgs::msg::Bool>(
-          "task_mode", rclcpp::QoS(10),
-          std::bind(&SaturnControllerInterface::taskModeCallback, this,
-                    std::placeholders::_1));
-
-    // 1, 六足机器人腿部里程计
-    leg_odom_publisher = this->create_publisher<nav_msgs::msg::Odometry>("leg_odom", 5);
-
-    // 2, robot_state(std_msgs::msg::UInt32): 机器人基本状态
-    // 0 -> standdown
-    // 1 -> standingup
-    // 2 -> standup
-    // 3 -> enable forcecontrol
-    // 4 -> motion
-    // 5 -> sitting down
-    // 6 -> fall down
-    robot_state_publisher = this->create_publisher<std_msgs::msg::UInt32>("robot_state", 10);
-
-    // 3, dock_state(std_msgs::msg::UInt32): 机器人上桩状态
-    // 0 -> idle
-    // 1 -> going to dock
-    // 2 -> stand above dock
-    // 3 -> adjust pose
-    // 4 -> lie on dock
-    // 5 -> standing after charging
-    // 6 -> leaving dock
-    // 7 -> leave dock
-    dock_state_publisher = this->create_publisher<std_msgs::msg::UInt32>("dock_state", 10);
-
-    // 4, 控制模式-手动、导航
-    // control_mode(std_msgs::msg::UInt8)：
-    // 0 -> 手持遥控
-    // １ -> 自动（导航）
-    nav_or_manual_publisher = this->create_publisher<std_msgs::msg::UInt8>("control_mode", 10);
-
-    // 5, gait(std_msgs::msg::UInt8): 机器人当前步态
-    // 0 -> 行走
-    // 1 -> 楼梯
-    // 2 -> 湿滑
-    // 3 -> 跑步
-    //saturn 设置六足机器人场景步态（1, 趴下, 2，行走步态， 3, 楼梯步态）
-    gait_scene_publisher = this->create_publisher<std_msgs::msg::UInt8>("gait", 10);
-
-    // 6, battery_state(sensor_msgs::msg::BatteryState)：电池电量信息,目前只有percentage字段有意义,
-    // 粗略代表机器人当前电量百分比
-    battery_state_publisher = this->create_publisher<sensor_msgs::msg::BatteryState>("battery_state", 5);
-
-    // 7, robot_mode 机器人模式，用于向巡检平台上报
-    // 0->空闲（默认）
-    // 1->手持遥控
-    // 2->任务
-    // 3->充电中
-    robot_mode_publisher = this->create_publisher<std_msgs::msg::Int32>("robot_mode", 10);
-
-    // 8，charge_state(std_msgs::msg::Bool)机器人在充电桩充电返回true，否则返回false
-    charge_state_publisher = this->create_publisher<std_msgs::msg::Bool>("charge_state", 10);
-
-    //9, 六足机器人IMU数据
-    imu_publisher = this->create_publisher<sensor_msgs::msg::Imu>("imu", 5);
-
-    // services 
-    // 1, robot_command (ysc_robot_msgs::srv::RobotCommand);
-    // 对机器人的基本控制接口，如切换步态，上下桩，起立，趴下
-    //当前电量、最高关节温度、最高驱动温度、六足机器人步态状态（趴下、行走步态、楼梯步态）
-    robot_command_service = this->create_service<ysc_robot_msgs::srv::RobotCommand>("robot_command", 
-            std::bind(&SaturnControllerInterface::RobotCommandCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    rc_version_service = this->create_service<saturn_msgs::srv::RcVersion>("rc_version", 
-            std::bind(&SaturnControllerInterface::RcVersionCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    driver_enable_service = this->create_service<saturn_msgs::srv::DriverEnable>("rc_driver_enable", 
-            std::bind(&SaturnControllerInterface::DriverEnableCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    height_scale_service = this->create_service<saturn_msgs::srv::HeightScale>("rc_height_scale", 
-            std::bind(&SaturnControllerInterface::HeightScaleCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    velocity_scale_service = this->create_service<saturn_msgs::srv::VelocityScale>("rc_velocity_scale", 
-            std::bind(&SaturnControllerInterface::VelocityScaleCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    //saturn 设置六足机器人场景步态（1, 趴下, 2，行走步态， 3, 楼梯步态）
-    gait_scene_service = this->create_service<saturn_msgs::srv::GaitScene>("rc_gait_scene", 
-            std::bind(&SaturnControllerInterface::setGaitSceneCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    auto_charge_service = this->create_service<saturn_msgs::srv::AutoCharge>("rc_auto_charge", 
-            std::bind(&SaturnControllerInterface::autoChargeCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    nav_or_manual_service = this->create_service<saturn_msgs::srv::NavOrManual>("rc_nav_or_manual", 
-            std::bind(&SaturnControllerInterface::navOrManualControlCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    guardian_switch_service = this->create_service<saturn_msgs::srv::GuardianSwitch>("guardian_switch", 
-            std::bind(&SaturnControllerInterface::guardianSwitchCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    guardian_service = this->create_service<daystar_navigation_msgs::srv::Guardian>("guardian", 
-            std::bind(&SaturnControllerInterface::guardianServiceCallback, this, 
-            std::placeholders::_1, std::placeholders::_2));
-
-    tip_position_wrt_hip_publisher = 
-            this->create_publisher<std_msgs::msg::Float32MultiArray>("tip_position_wrt_hip", 10);
-    touch_detection_publisher = 
-            this->create_publisher<std_msgs::msg::Float32MultiArray>("touch_detection", 10);
-
-    //当前电量、最高关节温度、最高驱动温度、六足机器人步态状态（趴下、行走步态、楼梯步态）
-    common_status_publisher = this->create_publisher<saturn_msgs::msg::CommonStatus>("rc_common_status", 5);
-    //yunshenchu 故障上报, 32位的数字，表示错误码
-    error_state_publisher = this->create_publisher<std_msgs::msg::Int32>("rc_error_state", 5);
-    //yunshenchu 六足机器人关节角度
-    joint_state_publisher = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 5);
-
-    //yunshenchu 六足机器人位姿 文档2.3 绝影四足机器人位姿调整（需要确认是发布还是订阅）
-    pose_estimate_sub = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-          "rc_pose_estimate", rclcpp::QoS(10),
-          std::bind(&SaturnControllerInterface::poseEstimateCallback, this,
-                    std::placeholders::_1));
-
-    this->declare_parameter<std::int32_t>("imu_sender_frequency", 1000);
-    this->get_parameter("imu_sender_frequency",imu_frquency);
-
-    this->declare_parameter<std::int32_t>("common_status_frequency", 20);
-    this->get_parameter("common_status_frequency",com_frquency);
-
-    RCLCPP_WARN(this->get_logger(), "SaturnControllerInterface timer --imu_frquency=[%d]  com_frquency=[%d]-", imu_frquency, com_frquency);
-    
-    run_imu_thread_ = true;
-    imu_thread_ = std::thread(&SaturnControllerInterface::ImuPublisherThread, this);
-
-    run_legged_odom_thread = true;
-    legged_odom_thread = std::thread(&SaturnControllerInterface::LeggedOdomPublisherThread, this);
-
-    run_common_state_thread_ = true;
-    common_state_thread_ = std::thread(&SaturnControllerInterface::CommonPublisherThread, this);
-    robot_mode_backup = -1;
-}
-
-SaturnControllerInterface::~SaturnControllerInterface() {
-    run_imu_thread_ = false;
-    run_common_state_thread_ = false;
-    run_legged_odom_thread = false;
-    imu_thread_.join();
-    common_state_thread_.join();
-    legged_odom_thread.join();
-}
-
-void SaturnControllerInterface::ImuPublisherThread()
-{
-    common::MC_MOTION_IMU_DATA imu_data;
-    rclcpp::Rate rate(350); //200hz
-    while(run_imu_thread_)
-    {
-        auto imu_message = sensor_msgs::msg::Imu();
-        rcClientInterfaceGetImuData(&imu_data);
-        // RCLCPP_WARN(this->get_logger(), 
-        //         "ImuPublisherThread=[%f] [%f] [%f] [%f] -",
-        //         imu_data.orientation.x, imu_data.orientation.y,
-        //         imu_data.orientation.z, imu_data.orientation.w);
-    
-        imu_message.header.stamp = rclcpp::Clock().now();
-        imu_message.header.frame_id = "imu";
-        
-        imu_message.orientation.x = imu_data.orientation.x;
-        imu_message.orientation.y = imu_data.orientation.y;
-        imu_message.orientation.z = imu_data.orientation.z;
-        imu_message.orientation.w = imu_data.orientation.w;
-
-        imu_message.angular_velocity.x = imu_data.angular_velocity.x;
-        imu_message.angular_velocity.y = imu_data.angular_velocity.y;
-        imu_message.angular_velocity.z = imu_data.angular_velocity.z;
-
-        imu_message.linear_acceleration.x = imu_data.linear_acceleration.x;
-        imu_message.linear_acceleration.y = imu_data.linear_acceleration.y;
-        imu_message.linear_acceleration.z = imu_data.linear_acceleration.z;
-
-        for(int i = 0; i < 9; i++){
-            imu_message.orientation_covariance[i] = imu_data.orientation_covariance[i];
-            imu_message.angular_velocity_covariance[i] = imu_data.angular_velocity_covariance[i];
-            imu_message.linear_acceleration_covariance[i] = imu_data.linear_acceleration_covariance[i];
-        }
-        imu_publisher->publish(imu_message); 
-        rate.sleep();
-    }
-    return;
-}
-
-void SaturnControllerInterface::LeggedOdomPublisherThread()
-{
-    rclcpp::Rate rate(200); //200hz
-    while(run_legged_odom_thread)
-    {
-        common::JETSON_VISUAL_FOOTHOLD_STATE foothold_state;
-        rcClientInterfaceGetVisualFootholdState(&foothold_state);
-
-        // RCLCPP_INFO(rclcpp::get_logger("test"), "time GOT %ld odom %f ", foothold_state.timestamp, foothold_state.robot_odometry.point.x);
-
-        //yunshenchu 六足机器人腿部里程计
-        auto odometry_data = nav_msgs::msg::Odometry();
-        // odometry_data.header.stamp = rclcpp::Clock().now();
-        int sec = foothold_state.timestamp / 1000000000;
-        int nanosec = foothold_state.timestamp % 1000000000;
-        odometry_data.header.stamp.sec = sec;
-        odometry_data.header.stamp.nanosec = nanosec;
-        // RCLCPP_INFO(rclcpp::get_logger("test"), "time debug %ld %ld %ld", foothold_state.timestamp, odometry_data.header.stamp.sec, odometry_data.header.stamp.nanosec);
-
-        odometry_data.header.frame_id = "odom";
-        odometry_data.child_frame_id = "base_link";
-        odometry_data.pose.pose.position.x = foothold_state.robot_odometry.point.x;
-        odometry_data.pose.pose.position.y = foothold_state.robot_odometry.point.y;
-        odometry_data.pose.pose.position.z = foothold_state.robot_odometry.point.z;
-        // odometry_data.pose.pose.orientation.x = com_status_data.odometry.quaternion.x;
-        // odometry_data.pose.pose.orientation.y = com_status_data.odometry.quaternion.y;
-        // odometry_data.pose.pose.orientation.z = com_status_data.odometry.quaternion.z;
-        // odometry_data.pose.pose.orientation.w = com_status_data.odometry.quaternion.w;
-        
-        // odometry_data.twist.twist.linear.x = com_status_data.odometry.twist.linear.x;
-        // odometry_data.twist.twist.linear.y = com_status_data.odometry.twist.linear.y;
-        // odometry_data.twist.twist.linear.z = com_status_data.odometry.twist.linear.z;
-        // odometry_data.twist.twist.angular.x = com_status_data.odometry.twist.angular.x;
-        // odometry_data.twist.twist.angular.y = com_status_data.odometry.twist.angular.y;
-        // odometry_data.twist.twist.angular.z = com_status_data.odometry.twist.angular.z;
-        leg_odom_publisher->publish(odometry_data);
-        rate.sleep();
-    }
-}
-
-void SaturnControllerInterface::CommonPublisherThread()
-{
-    rclcpp::Rate rate(50);
-    while(run_common_state_thread_)
-    {
-        common::ROBOT_COMMON_STATUS com_status_data;
-        // RCLCPP_WARN(this->get_logger(), "CommonPublisherThread 00 ");
-        rcClientInterfaceGetCommonStatus(&com_status_data);
-        //发布机器人状态信息。目前先和imu放在同一个线程中。
-        auto common_message = saturn_msgs::msg::CommonStatus();
-        common_message.network = com_status_data.network;
-        common_message.belie_or_stand = com_status_data.belie_or_stand;
-
-        auto pub_robot_state = std_msgs::msg::UInt32();
-        if(common_message.belie_or_stand == robot_control::common::BODY_POSTURE_STATE::LIEDOWN)
-        {
-            pub_robot_state.data = 5;
-        }
-        else if(common_message.belie_or_stand == robot_control::common::BODY_POSTURE_STATE::STANDUP)
-        {
-            pub_robot_state.data = 2;
-        }
-        else
-        {
-            pub_robot_state.data = -1;
-        }
-        robot_state_publisher->publish(pub_robot_state);
-
-        //临时逻辑 todo
-        auto pub_dock_state = std_msgs::msg::UInt32();
-        if(common_message.belie_or_stand == robot_control::common::CHARGE_SWITCH_STATE::ENTER_SUCCEEDED)
-        {
-            pub_dock_state.data = 4;
-        }
-        else if((common_message.belie_or_stand == robot_control::common::CHARGE_SWITCH_STATE::ENTER_FAILED)
-                || ((common_message.belie_or_stand == robot_control::common::CHARGE_SWITCH_STATE::EXIT_SUCCEEDED)))
-        {
-            pub_dock_state.data = 0;
-        }
-        dock_state_publisher->publish(pub_dock_state);
-
-        common_message.emergency = com_status_data.emergency;
-        common_message.avoidance = com_status_data.avoidance;
-        common_message.heartbeat = com_status_data.heartbeat;
-        robot_heartbeat = com_status_data.heartbeat;
-        common_message.cur_speed = com_status_data.cur_speed;
-        common_message.cur_height = com_status_data.cur_height;
-        // common_message.position = com_status_data.position;
-        common_message.max_height = com_status_data.max_height;
-        common_message.max_speed = com_status_data.max_speed;
-        common_message.cur_scene_switch_state = com_status_data.cur_scene_switch_state;
-
-        common_message.joy_mode = com_status_data.joy_mode;
-        auto nav_or_manual_state = std_msgs::msg::UInt8();
-        if(common_message.joy_mode == robot_control::common::NAV_OR_JOY_MODE::nav_control)
-        {
-            nav_or_manual_state.data = 1;
-        }else if(common_message.joy_mode == robot_control::common::NAV_OR_JOY_MODE::joy_control)
-        {
-            nav_or_manual_state.data = 0;
-        }
-        nav_or_manual_publisher->publish(nav_or_manual_state);
-
-        common_message.cur_scene = com_status_data.cur_scene;
-        robot_cur_scene = com_status_data.cur_scene;
-        auto gait_scene_state = std_msgs::msg::UInt8();
-        if((common_message.cur_scene == robot_control::common::SCENE_TYPE::WALKING)
-                || (common_message.cur_scene == robot_control::common::SCENE_TYPE::CHARGE))
-        {
-            gait_scene_state.data = 0;
-        }
-        else if(common_message.cur_scene == robot_control::common::SCENE_TYPE::STAIRS)
-        {
-            gait_scene_state.data = 1;
-        }
-        else
-        {
-            gait_scene_state.data = -1;
-        }
-        gait_scene_publisher->publish(gait_scene_state);
-
-        auto tip_position_wrt_hip_message = std_msgs::msg::Float32MultiArray();
-        for(int i = 0; i < LEG_NUMBER*3; i++)
-        {
-            // common_message.tip_position_wrt_hip[i] =com_status_data.tip_position_wrt_hip[i];
-            // tip_position_wrt_hip_message.data[i] =com_status_data.tip_position_wrt_hip[i];
-        }
-
-        auto touch_detection_message = std_msgs::msg::Float32MultiArray();
-        for(int i = 0; i < LEG_NUMBER; i++)
-        {
-            // common_message.touch_detection[i] =com_status_data.touch_detection[i];
-            // touch_detection_message.data[i] =com_status_data.touch_detection[i];
-        }
-
-        // tip_position_wrt_hip_publisher->publish(tip_position_wrt_hip_message);
-        // touch_detection_publisher->publish(touch_detection_message);
-        // // common_message.charge_status.battery_info.power = com_status_data.charge_status.battery_info.power;
-
-        float battery_power = 0.0;
-        battery_power = com_status_data.charge_status.battery_info.level;
-        if(battery_power < 0)
-        {
-            battery_power = 0;
-        }
-        else if(battery_power > 1)
-        {
-            battery_power = 1;
-        }
-        auto battery_state_state = sensor_msgs::msg::BatteryState();
-        battery_state_state.percentage = battery_power;
-        battery_state_publisher->publish(battery_state_state);
-
-        auto robot_mode_state = std_msgs::msg::Int32();
-        
-        if((cur_robot_is_idle == true) && (nav_or_manual_state.data == 1))
-        {
-            robot_mode_state.data = 0;
-        }
-        else if(nav_or_manual_state.data == 0)
-        {
-            robot_mode_state.data = 1;
-        }
-        else if((cur_robot_is_idle == false) && (nav_or_manual_state.data == 1))
-        {
-            robot_mode_state.data = 2;
-        }
-        else if(common_message.cur_scene == robot_control::common::SCENE_TYPE::CHARGE)
-        {
-            robot_mode_state.data = 3;
-        }
-        else
-        {
-            robot_mode_state.data = 0;
-        }
-        if(robot_mode_backup != robot_mode_state.data)
-        {
-            robot_mode_publisher->publish(robot_mode_state);
-        }
-        robot_mode_backup = robot_mode_state.data;
-
-        auto pub_charge_state = std_msgs::msg::Bool();
-        if(common_message.belie_or_stand == robot_control::common::AUTO_CHARGE_STATE::charging)
-        {
-            pub_charge_state.data = true;
-        }
-        else
-        {
-            pub_charge_state.data = false;
-        }
-        charge_state_publisher->publish(pub_charge_state);
-
-
-        common_status_publisher->publish(common_message);
-
-        auto error_data = std_msgs::msg::Int32();
-        error_data.data = com_status_data.error_code;
-        //yunshenchu 故障上报, 32位的数字，表示错误码
-        error_state_publisher->publish(error_data);
-
-        common::ROBOT_JOINTS_STATUS joint_status_data;
-        rcClientInterfaceGetJointsStatus(&joint_status_data);
-        auto joints_state_message = sensor_msgs::msg::JointState();
-        joints_state_message.header.stamp = rclcpp::Clock().now();
-
-        joints_state_message.name.resize(18); //指定name数组长度
-        joints_state_message.position.resize(18);//指定position数组长度
-        joints_state_message.velocity.resize(18);//指定velocity数组长度
-        joints_state_message.effort.resize(18);//指定effort数组长度
-        for(int i = 0; i < 18; i++)
-        {
-            joints_state_message.name[i] ="joint" + std::to_string(i);//name数组的第一个元素赋值
-            joints_state_message.position[i] = joint_status_data.joints[i].position;//position数组的第一个元素赋值
-            joints_state_message.velocity[i] = joint_status_data.joints[i].velocity;//velocity数组的第一个元素赋值
-            joints_state_message.effort[i] = joint_status_data.joints[i].effort;//effort数组的第一个元素赋值
-        }
-        //yunshenchu 六足机器人关节角度
-        joint_state_publisher->publish(joints_state_message);
-        rate.sleep();
-    }
-    return;
-}
-
-void SaturnControllerInterface::cmdVelCallback(
-    const geometry_msgs::msg::Twist::SharedPtr vel_msg)
-{
-    common::ROBOT_TWIST rc_direct;
-    rc_direct.linear.x = vel_msg->linear.x * guardian_velocity_decay_ratio_;
-    rc_direct.linear.y = vel_msg->linear.y;
-    rc_direct.linear.z = vel_msg->linear.z;
-    rc_direct.angular.x = vel_msg->angular.x;
-    rc_direct.angular.y = vel_msg->angular.y;
-    rc_direct.angular.z = vel_msg->angular.z;
-    int64_t time_millis;
-    rcClientInterfaceDirectionMovement(
-                robot_control::common::NAV_OR_JOY_MODE::nav_control,
-                rc_direct, robot_heartbeat);
-    return;
-}
-
-void SaturnControllerInterface::tagDetectedCallback(
-        const std_msgs::msg::Bool::SharedPtr tag_detected)
-{
-    common::JETSON_CHARGE_COMMAND charge_command;
-    if(tag_detected->data == true)
-    {
-        charge_command.tag_state = true;
-    }
-    else
-    {
-        charge_command.tag_state = false;
-    }
-    rcClientInterfaceAutoRechargeCmd(robot_control::common::NAV_OR_JOY_MODE::nav_control, charge_command);
-    return;
-}
-
-void SaturnControllerInterface::poseInTagCallback(
-        const geometry_msgs::msg::PoseStamped::SharedPtr pose_in_tag)
-{
-    common::JETSON_CHARGE_COMMAND charge_command;
-    charge_command.position.x = pose_in_tag->pose.orientation.x;
-    charge_command.position.y = pose_in_tag->pose.orientation.y;
-    charge_command.position.z = pose_in_tag->pose.orientation.z;
-    charge_command.position.w = pose_in_tag->pose.orientation.w;
-    rcClientInterfaceAutoRechargeCmd(robot_control::common::NAV_OR_JOY_MODE::nav_control, charge_command);
-    return;
-}
-
-void SaturnControllerInterface::guardianCallback(
-        const daystar_navigation_msgs::msg::Guardian::SharedPtr guardian_msg)
-{
-    float velocity_decay_ratio = guardian_msg->velocity_decay_ratio;
-    std::string trigger_source = guardian_msg->trigger_source;
-    rcClientInterfaceSetGuardian(robot_control::common::NAV_OR_JOY_MODE::nav_control, velocity_decay_ratio, trigger_source);
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "guardianCallback Incoming request: %f %s", guardian_msg->velocity_decay_ratio, guardian_msg->trigger_source.c_str());
-    return;
-}
-
-void SaturnControllerInterface::taskModeCallback(
-            const std_msgs::msg::Bool::SharedPtr task_mode)
-{
-    //不支持
-    if(task_mode->data == true)
-    {  
-        cur_robot_is_idle = true;
-    }else
-    {
-        cur_robot_is_idle = false;
-    }
-    return;
-}
-
-void SaturnControllerInterface::poseEstimateCallback(
-                const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr robot_pose) 
-{
-    // rcClientInterfaceBodyHighAdjust(high_scale->scale);
-    common::ROBOT_POSE_ESTIMATE estimate;
-    estimate.point.x = robot_pose->pose.pose.position.x;
-    estimate.point.y = robot_pose->pose.pose.position.y;
-    estimate.point.z = robot_pose->pose.pose.position.z;
-    estimate.quaternion.x = robot_pose->pose.pose.orientation.x;
-    estimate.quaternion.y = robot_pose->pose.pose.orientation.y;
-    estimate.quaternion.z = robot_pose->pose.pose.orientation.z;
-    estimate.quaternion.w = robot_pose->pose.pose.orientation.w;
-    rcClientInterfaceSetPoseEstimate(robot_control::common::NAV_OR_JOY_MODE::nav_control, estimate);
-    return;
-}
-
-//todo
-void SaturnControllerInterface::RobotCommandCallback(
-                const ysc_robot_msgs::srv::RobotCommand::Request::SharedPtr request,
-                  ysc_robot_msgs::srv::RobotCommand::Response::SharedPtr response)
-{
-    
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request: %s", request->comamnd_name.c_str());
-    bool result = true;
-    std::string command = request->comamnd_name;
-
-    if ("GoToDock" == request->comamnd_name) 
-    {
-        result = rcClientInterfaceEnterOrExitCharge(robot_control::common::NAV_OR_JOY_MODE::nav_control, true);
-    }
-    else if ("LeaveDock" == request->comamnd_name) 
-    {
-        result = rcClientInterfaceEnterOrExitCharge(robot_control::common::NAV_OR_JOY_MODE::nav_control, false);
-    }
-    else if ("HeartBeat" == request->comamnd_name) 
-    {
-
-    }
-    else if ("Stop" == request->comamnd_name) 
-    {
-        common::ROBOT_TWIST rc_direct;
-        rc_direct.linear.x = 0.0;
-        rc_direct.linear.y = 0.0;
-        rc_direct.linear.z = 0.0;
-        rc_direct.angular.x = 0.0;
-        rc_direct.angular.y = 0.0;
-        rc_direct.angular.z = 0.0;
-        result = rcClientInterfaceDirectionMovement(
-                robot_control::common::NAV_OR_JOY_MODE::nav_control,
-                rc_direct, robot_heartbeat);
-    } 
-    else if ("StandUpDown" == request->comamnd_name)
-    {
-        if((robot_cur_scene == robot_control::common::SCENE_TYPE::WALKING)
-            ||(robot_cur_scene == robot_control::common::SCENE_TYPE::STAIRS))
-        {
-            result = rcClientInterfaceSetScene(robot_control::common::NAV_OR_JOY_MODE::nav_control, common::SCENE_TYPE::LIE_DOWN);
-        }else if (robot_cur_scene == robot_control::common::LIE_DOWN)
-        {
-            result = rcClientInterfaceSetScene(robot_control::common::NAV_OR_JOY_MODE::nav_control, common::SCENE_TYPE::WALKING);
-        }
-    }
-    else if ("EnableNavigation" == request->comamnd_name) 
-    {
-        common::NAV_OR_JOY_MODE nav_joy;
-        nav_joy = common::NAV_OR_JOY_MODE::nav_control;
-        result = rcClientInterfaceSetNavOrJoyControl(nav_joy);
-    }
-    else if ("EnableTeleoption" == request->comamnd_name) 
-    {
-        common::NAV_OR_JOY_MODE nav_joy;
-        nav_joy = common::NAV_OR_JOY_MODE::joy_control;
-        result = rcClientInterfaceSetNavOrJoyControl(nav_joy);
-    }
-    else if ("StairsGait" == request->comamnd_name)
-    {
-        if(robot_cur_scene != robot_control::common::STAIRS) 
-        {
-            result = rcClientInterfaceSetScene(robot_control::common::NAV_OR_JOY_MODE::nav_control, common::SCENE_TYPE::STAIRS);
-        }
-    }
-    else if ("OrdinaryGait" == request->comamnd_name)
-    {
-        if (robot_cur_scene != robot_control::common::SCENE_TYPE::WALKING) 
-        {
-            result = rcClientInterfaceSetScene(robot_control::common::NAV_OR_JOY_MODE::nav_control, common::SCENE_TYPE::WALKING);
-        }
-    }
-
-    response->result = result;
-    return;
-}
-
-void SaturnControllerInterface::RcVersionCallback(const saturn_msgs::srv::RcVersion::Request::SharedPtr request,
-                saturn_msgs::srv::RcVersion::Response::SharedPtr response)
-{
-    response->version = rcClientInterfaceGetMcVersion(request->type);
-
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request---: %d", request->type);
-    return;
-}
-
-void SaturnControllerInterface::DriverEnableCallback(const saturn_msgs::srv::DriverEnable::Request::SharedPtr request,
-                saturn_msgs::srv::DriverEnable::Response::SharedPtr response)
-{
-    response->res = rcClientInterfaceDriverEnable(robot_control::common::NAV_OR_JOY_MODE::nav_control, request->enable);
-
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request-: %d", request->enable);
-    return;
-}
-
-void SaturnControllerInterface::HeightScaleCallback(const saturn_msgs::srv::HeightScale::Request::SharedPtr request,
-                saturn_msgs::srv::HeightScale::Response::SharedPtr response)
-{
-    int scale = request->scale;
-    if(scale < 0)
-    {
-        scale = 0;
-    }
-    else if(scale > 100)
-    {
-        scale = 100;
-    }
-    response->res = rcClientInterfaceBodyHighAdjust(robot_control::common::NAV_OR_JOY_MODE::nav_control, scale);
-
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request: %d", request->scale);
-    return;
-}
-
-void SaturnControllerInterface::VelocityScaleCallback(const saturn_msgs::srv::VelocityScale::Request::SharedPtr request,
-                saturn_msgs::srv::VelocityScale::Response::SharedPtr response)
-{
-    int scale = request->scale;
-    if(scale < 0)
-    {
-        scale = 0;
-    }
-    else if(scale > 100)
-    {
-        scale = 100;
-    }
-    response->res = rcClientInterfaceSpeedAdjust(robot_control::common::NAV_OR_JOY_MODE::nav_control, scale);
-
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request: %d", request->scale);
-    return;
-}
-
-void SaturnControllerInterface::setGaitSceneCallback(const saturn_msgs::srv::GaitScene::Request::SharedPtr request,
-                saturn_msgs::srv::GaitScene::Response::SharedPtr response)
-{
-    int scene = request->scene;
-
-    if((scene > common::SCENE_TYPE::NULL_SCENE) && (scene <= common::SCENE_TYPE::STAIRS))
-    {
-        response->res = rcClientInterfaceSetScene(robot_control::common::NAV_OR_JOY_MODE::nav_control, (common::SCENE_TYPE)scene);
-    }
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request: %d", request->scene);
-    return;
-}
-
-void SaturnControllerInterface::autoChargeCallback(const saturn_msgs::srv::AutoCharge::Request::SharedPtr request,
-                saturn_msgs::srv::AutoCharge::Response::SharedPtr response)
-{
-    int auto_charge = request->method;
-
-    if((auto_charge > 0) && (auto_charge <= common::AUTO_CHARGE_STATE::charge_finished))
-    {
-        response->res = rcClientInterfaceSetAutoCharge(robot_control::common::NAV_OR_JOY_MODE::nav_control, (common::AUTO_CHARGE_STATE)auto_charge);
-    }
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request: %d",request->method);
-    return;
-}
-
-
-void SaturnControllerInterface::navOrManualControlCallback(const saturn_msgs::srv::NavOrManual::Request::SharedPtr request,
-                saturn_msgs::srv::NavOrManual::Response::SharedPtr response)
-{
-    int control_mode = request->method;
-
-    if((control_mode > 0) && (control_mode <= common::NAV_OR_JOY_MODE::joy_control))
-    {
-        response->res = rcClientInterfaceSetNavOrJoyControl((common::NAV_OR_JOY_MODE)control_mode);
-    }
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request: %d", request->method);
-    return;
-}
-
-void SaturnControllerInterface::guardianSwitchCallback(const saturn_msgs::srv::GuardianSwitch::Request::SharedPtr request,
-                saturn_msgs::srv::GuardianSwitch::Response::SharedPtr response)
-{
-    bool enable = request->enable;
-
-    response->res = rcClientInterfaceSetGuardianSwitch(robot_control::common::NAV_OR_JOY_MODE::nav_control, enable);
-    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "guardianSwitchCallback Incoming request: %d", request->enable);
-    return;
-}
-
-void SaturnControllerInterface::guardianServiceCallback(const daystar_navigation_msgs::srv::Guardian::Request::SharedPtr request,
-                daystar_navigation_msgs::srv::Guardian::Response::SharedPtr response)
-{
-    if(request->velocity_decay_ratio <= 0)
-    {
-        this->guardian_velocity_decay_ratio_ = 0.0;
-        //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "guardian robot stops");
-    }
-    else if(request->velocity_decay_ratio < 1.0 && request->velocity_decay_ratio > 0)
-    {
-        this->guardian_velocity_decay_ratio_ = request->velocity_decay_ratio;
-        //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "guardian robot slows down");
-    }
-    else
-    {
-        this->guardian_velocity_decay_ratio_ = 1.0;
-        //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "guardian deactivated");
-    }
-    // bool enable = request->enable;
-
-    // response->res = rcClientInterfaceSetGuardianSwitch(robot_control::common::NAV_OR_JOY_MODE::nav_control, enable);
-    // // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "guardianSwitchCallback Incoming request: %d", request->enable);
-    return;
-}
-
-} 
-
-RCLCPP_COMPONENTS_REGISTER_NODE(saturn_controller_interface::SaturnControllerInterface)

src/saturn_controller_interface/src/saturn_controller_node.cpp

@@ -1,16 +0,0 @@
-#include <memory>
-
-#include <rclcpp/rclcpp.hpp>
-
-#include "saturn_controller_interface/saturn_controller_interface.hpp"
-
-int main(int argc, char *argv[])
-{
-  rclcpp::init(argc, argv);
-
-  rclcpp::spin(std::make_unique<saturn_controller_interface::SaturnControllerInterface>(rclcpp::NodeOptions()));
-
-  rclcpp::shutdown();
-
-  return 0;
-}

src/saturn_controller_interface/urdf/saturn_base_imu_rslidar_tf2.urdf.xml

@@ -1,68 +0,0 @@
-<?xml version="1.0"?>
-<robot name="saturn">
-  <link name="base_link">
-    <visual>
-      <geometry>
-        <box size="0.805 0.320 0.160"/>
-      </geometry>
-    </visual>
-  </link>
-
-  <link name="imu">
-    <visual>
-      <geometry>
-        <box size="0.05 0.04 0.015"/>
-      </geometry>
-    </visual>
-  </link>
-
-  <link name="rslidar">
-    <visual>
-      <geometry>
-        <cylinder length="0.12" radius="0.06"/>
-      </geometry>
-    </visual>
-  </link>
-
-  <link name="camera">
-    <visual>
-      <geometry>
-        <cylinder length="0.01" radius="0.01"/>
-      </geometry>
-    </visual>
-  </link>
-
-  <joint name="base_to_imu" type="fixed">
-    <parent link="base_link"/>
-    <child link="imu"/>
-    <origin xyz="0.2847 0.0 0.042"/>
-  </joint>
-
-  <joint name="base_to_rslidar" type="fixed">
-    <parent link="base_link"/>
-    <child link="rslidar"/>
-    <origin xyz="0.4082 0.0 0.221"/>
-  </joint>
-
-  <joint name="base_to_camera" type="fixed">
-    <parent link="base_link"/>
-    <child link="camera"/>
-    <origin xyz="0.51763 -0.061375 0.0398"/>
-  </joint>
-  
-  <link name="livox_frame">
-    <visual>
-      <geometry>
-        <cylinder length="0.12" radius="0.06"/>
-      </geometry>
-    </visual>
-  </link>
-  
-  <joint name="base_to_livox" type="fixed">
-    <parent link="base_link"/>
-    <child link="livox_frame"/>
-    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-    <!--origin xyz="0.1751 0.0 0.1258" rpy="1.57 0.0 -1.57"/>-->
-  </joint>
-  
-</robot>