├── message_transformer └── message_transformer │ ├── script │ ├── log.sh │ ├── status.sh │ ├── start.sh │ └── stop.sh │ ├── msg │ ├── SimpleCMD.msg │ └── ComplexCMD.msg │ ├── package.xml │ ├── launch │ └── message_transformer.launch │ ├── include │ ├── sensor_logger.h │ └── protocol.h │ ├── CMakeLists.txt │ └── src │ ├── ros2qnx.cpp │ ├── sensor_checker.cpp │ ├── qnx2ros.cpp │ └── nx2app.cpp ├── LICENSE ├── README_ZH.md └── README.md /message_transformer/message_transformer/script/log.sh: -------------------------------------------------------------------------------- 1 | sudo journalctl -fu transfer.service 2 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/script/status.sh: -------------------------------------------------------------------------------- 1 | sudo systemctl status transfer.service 2 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/msg/SimpleCMD.msg: -------------------------------------------------------------------------------- 1 | int32 cmd_code 2 | int32 cmd_value 3 | int32 type -------------------------------------------------------------------------------- /message_transformer/message_transformer/msg/ComplexCMD.msg: -------------------------------------------------------------------------------- 1 | int32 cmd_code 2 | int32 cmd_value 3 | int32 type 4 | float64 data -------------------------------------------------------------------------------- /message_transformer/message_transformer/script/start.sh: -------------------------------------------------------------------------------- 1 | sudo systemctl start transfer.service 2 | sudo systemctl is-active transfer.service 3 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/script/stop.sh: -------------------------------------------------------------------------------- 1 | sudo systemctl stop transfer.service 2 | sudo systemctl is-active transfer.service 3 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | message_transformer 4 | 1.0.0 5 | message_transformer 6 | TODO 7 | Fan Zhanhao 8 | Fan Zhanhao 9 | 10 | catkin 11 | roscpp 12 | rospy 13 | sensor_msgs 14 | std_msgs 15 | tf 16 | message_generation 17 | message_runtime 18 | roscpp 19 | rospy 20 | sensor_msgs 21 | std_msgs 22 | tf 23 | 24 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 DeepRoboticsLab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/launch/message_transformer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/include/sensor_logger.h: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/Bool.h" 3 | 4 | class SensorLogger{ 5 | public: 6 | SensorLogger(ros::NodeHandle nh, std::string sensor_name) : sensor_name_(sensor_name){ 7 | sub_ = nh.subscribe("/sensor_status/" + sensor_name + "_isalive", 1, &SensorLogger::sensor_stat_set, this); 8 | } 9 | 10 | void sensor_stat_set(const std_msgs::Bool::ConstPtr& msg) 11 | { 12 | isalive_ = msg->data; 13 | } 14 | 15 | bool isalive_=false; 16 | 17 | private: 18 | ros::Subscriber sub_; 19 | std::string sensor_name_; 20 | }; 21 | 22 | class SensorsLogger 23 | { 24 | public: 25 | std::unique_ptr imu_; 26 | std::unique_ptr odom_; 27 | std::unique_ptr odom2_; 28 | std::unique_ptr joint_; 29 | std::unique_ptr realsense_; 30 | std::unique_ptr lidar_; 31 | std::unique_ptr ultrasound_; 32 | 33 | SensorsLogger(ros::NodeHandle &nh){ 34 | imu_ = std::make_unique(nh, "imu"); 35 | odom_ = std::make_unique(nh, "odom"); 36 | odom2_ = std::make_unique(nh, "odom2"); 37 | joint_ = std::make_unique(nh, "joint"); 38 | realsense_ = std::make_unique(nh, "realsense"); 39 | lidar_ = std::make_unique(nh, "lidar"); 40 | ultrasound_ = std::make_unique(nh, "ultrasound"); 41 | } 42 | private: 43 | bool isdebug_=false; 44 | }; -------------------------------------------------------------------------------- /message_transformer/message_transformer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(message_transformer) 3 | 4 | SET(CMAKE_CXX_FLAGS "-std=c++14 -g -O3 ${CMAKE_CXX_FLAGS}") 5 | set(CMAKE_BUILD_TYPE Release) 6 | ## Find catkin macros and libraries 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | rospy 11 | geometry_msgs 12 | nav_msgs 13 | sensor_msgs 14 | std_msgs 15 | tf 16 | message_generation 17 | ) 18 | 19 | add_message_files( 20 | FILES 21 | SimpleCMD.msg 22 | ComplexCMD.msg 23 | ) 24 | 25 | generate_messages( 26 | DEPENDENCIES 27 | std_msgs 28 | ) 29 | 30 | catkin_package( 31 | CATKIN_DEPENDS 32 | roscpp 33 | rospy 34 | sensor_msgs 35 | std_msgs 36 | message_runtime 37 | tf 38 | ${PCL_OPENMP_PACKAGES} 39 | DEPENDS 40 | ) 41 | 42 | include_directories( 43 | include 44 | ${catkin_INCLUDE_DIRS} 45 | ) 46 | 47 | add_executable(qnx2ros 48 | src/qnx2ros.cpp 49 | ) 50 | add_dependencies(qnx2ros 51 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 52 | ${catkin_EXPORTED_TARGETS} 53 | ) 54 | target_link_libraries(qnx2ros 55 | ${catkin_LIBRARIES} 56 | ${PCL_LIBRARIES} 57 | ) 58 | 59 | add_executable(ros2qnx 60 | src/ros2qnx.cpp 61 | ) 62 | add_dependencies(ros2qnx 63 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 64 | ${catkin_EXPORTED_TARGETS} 65 | ) 66 | target_link_libraries(ros2qnx 67 | ${catkin_LIBRARIES} 68 | ) 69 | 70 | add_executable(nx2app 71 | src/nx2app.cpp 72 | ) 73 | add_dependencies(nx2app 74 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 75 | ${catkin_EXPORTED_TARGETS} 76 | ) 77 | target_link_libraries(nx2app 78 | ${catkin_LIBRARIES} 79 | ${PCL_LIBRARIES} 80 | ) 81 | 82 | add_executable(sensor_checker 83 | src/sensor_checker.cpp 84 | ) 85 | add_dependencies(sensor_checker 86 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 87 | ${catkin_EXPORTED_TARGETS} 88 | ) 89 | target_link_libraries(sensor_checker 90 | ${catkin_LIBRARIES} 91 | ) 92 | 93 | install(TARGETS nx2app qnx2ros ros2qnx sensor_checker 94 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 95 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 96 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 97 | ) 98 | 99 | install(DIRECTORY ./launch 100 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 101 | ) 102 | 103 | install(DIRECTORY ./script 104 | DESTINATION ${CMAKE_INSTALL_PREFIX} 105 | ) 106 | 107 | 108 | -------------------------------------------------------------------------------- /README_ZH.md: -------------------------------------------------------------------------------- 1 | # 绝影Lite3感知开发 2 | 3 | [English](./README.md) 4 | 5 | ## 运动通信功能包`message_transformer` 6 | 7 | ### 功能介绍 8 | 9 | 本案例实现了ROS与UDP消息的功能转换。 10 | 11 | 绝影Lite3的运动主机与感知主机之间、感知主机与手柄App之间的数据传输均采用UDP协议，通过本案例提供的***message_transformer_cpp*** 软件包，可实现： 12 | 13 | - 将运动主机上报的UDP消息转换为ROS话题进行发布，将感知主机下发的运动控制指令使用UDP发送给运动主机； 14 | - 接收App发送的控制指令，以开启和关闭感知主机上的AI功能。 15 | 16 | 17 | 其提供的ROS通信接口： 18 | 19 | **发布话题:** 运动主机向感知主机传输数据 20 | 21 | ```html 22 | 腿部里程计： /leg_odom (nav_msgs::Odometry) 23 | IMU数据： /imu/data (sensor_msgs::Imu) 24 | 关节数据： /joint_states (sensor_msgs::JointState) 25 | ``` 26 | 27 | **订阅话题：** 感知主机向运动主机传输数据 28 | 29 | ```html 30 | 速度指令： /cmd_vel (geometry_msgs::Twist) 31 | ``` 32 | 33 | 34 | ### 使用方法 35 | 36 | 1. 打开一个新的终端依次执行以下命令，以**启动通信功能包节点** `nx2app`、`rk2ros`、`ros2rk`： 37 | 38 | ```bash 39 | cd message_transformer_ws/ #进入功能包工作空间（/home/ysc/message_transformer_ws） 40 | source devel/setup.bash #添加工作空间环境变量 41 | roslaunch message_transformer message_transformer.launch #启动通信功能包节点 42 | ``` 43 | 44 | 2. 打开一个新的终端，使用ROS中的`rostopic`命令**查看机器狗状态信息**： 45 | 46 | ```bash 47 | rostopic info xxxxxx 48 | rostopic echo xxxxxx # xxxxxx指的是具体话题名称，可在自己的代码中订阅话题进行二次开发 49 | ``` 50 | 51 | 3. 使用`/cmd_vel`话题**向运动主机下发速度指令**，话题消息类型`geometry_msgs/Twist`定义如下： 52 | 53 | ```bash 54 | geometry_msgs/Vector3 linear # 线速度(m/s) 55 | float64 x # 前向速度，向前为正 56 | float64 y # 侧向速度，向左为正 57 | float64 z # 无效参数 58 | geometry_msgs/Vector3 angular # 角速度(rad/s) 59 | float64 x # 无效参数 60 | float64 y # 无效参数 61 | float64 z # 转向角速度，左转为正 62 | ``` 63 | - 用户可在基于ROS编译的C++和Python程序中发布该话题(需要用户具有ROS基础，ROS基础的学习请参考 http://wiki.ros.org/ROS/Tutorials )，也可以打开一个终端，输入命令发布进行调试，请先输入： 64 | 65 | ```bash 66 | rostopic pub /cmd_vel geometry_msgs/Twist 67 | ``` 68 | 69 | - 输入完先不运行，在语句后面加一个空格，再按Tab键，就会自动补充当前消息类型的内容，具体如下： 70 | 71 | ```bash 72 | rostopic pub /cmd_vel geometry_msgs/Twist "linear: 73 | x: 0.0 74 | y: 0.0 75 | z: 0.0 76 | angular: 77 | x: 0.0 78 | y: 0.0 79 | z: 0.0 80 | " 81 | ``` 82 | - 使用键盘上的向左/向右方向键移动光标，对速度值进行修改，然后在`geometry_msgs/Twist`之后加入` -r 10` 规定发布频率（即每秒发布10次），输入完毕后终端内容如下所示： 83 | 84 | ```bash 85 | rostopic pub /cmd_vel geometry_msgs/Twist -r 10 "linear: 86 | x: 0.2 87 | y: 0.1 88 | z: 0.0 89 | angular: 90 | x: 0.0 91 | y: 0.0 92 | z: 0.3 93 | " 94 | ``` 95 | 96 | - 运行命令，即可发布话题。 97 | 98 | - 传输程序会订阅该话题，并将其转为UDP指令消息发给运动主机。 99 | 100 | - 在传输程序已正常开启的情况下，用App切入自动模式，机器狗即可按照如上速度行动，为防止在调试过程中对人或物品造成损伤，请在空旷处调试，并随时准备将机器狗切回手动模式进行接管。 101 | 102 | ### 程序结构 103 | 104 | ```bash 105 | ~/message_transformer_ws/src/message_transformer 106 | └── message_transformer 107 | ├── CMakeLists.txt 108 | ├── include 109 | │ ├── protocol.h 110 | │ └── sensor_logger_.h 111 | ├── launch 112 | │ └── message_transformer.launch 113 | ├── msg 114 | │ ├── ComplexCMD.msg 115 | │ └── SimpleCMD.msg 116 | ├── package.xml 117 | ├── script 118 | │ ├── log.sh 119 | │ ├── start.sh 120 | │ ├── status.sh 121 | │ ├── stop.sh 122 | └── src 123 | ├── nx2app.cpp 124 | ├── qnx2ros.cpp 125 | ├── ros2qnx.cpp 126 | └── sensor_checker.cpp 127 | ``` 128 | 129 | - ***nx2app.cpp***主要用于感知主机与手柄App之间进行UDP通信，App下发指令码到感知主机，该程序根据收到的命令执行相应的操作。 130 | 131 | - ***qnx2ros.cpp***用于接收运动主机上报的数据，并将其转化为ROS话题，供其他功能包调用。 132 | 133 | - ***ros2qnx.cpp***订阅其他功能包节点发布的话题，转化为UDP数据报下发给运动主机。 134 | 135 | - ***sensor_checker.cpp***用于检查机器狗传感器的状态，并提示警告出现错误的传感器。 136 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/src/ros2qnx.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "message_transformer/SimpleCMD.h" 27 | #include "message_transformer/ComplexCMD.h" 28 | #include "../include/protocol.h" 29 | #include "../include/sensor_logger.h" 30 | 31 | using namespace std; 32 | 33 | class ROS2QNX 34 | { 35 | public: 36 | ROS2QNX() 37 | { 38 | fd_ = socket(AF_INET, SOCK_DGRAM,0); 39 | if(fd_==-1){ 40 | ROS_WARN("scoket create failed!"); 41 | } 42 | 43 | addr_qnx_.sin_family = AF_INET; 44 | addr_qnx_.sin_port = htons(43893); 45 | addr_qnx_.sin_addr.s_addr = inet_addr("192.168.1.120"); 46 | } 47 | 48 | void CmdVelCallback(geometry_msgs::TwistConstPtr msg){ 49 | int nbytes; 50 | ComplexCMD complexcmd; 51 | complexcmd.cmd_code = 320; 52 | complexcmd.cmd_value = 8; 53 | complexcmd.type = 1; 54 | complexcmd.data = msg->linear.x; ///< linear x velocity 55 | nbytes = sendto(fd_, &complexcmd, sizeof(complexcmd), 0, 56 | (struct sockaddr *)&addr_qnx_, sizeof(addr_qnx_)); 57 | 58 | complexcmd.cmd_code =325; 59 | complexcmd.cmd_value = 8; 60 | complexcmd.type = 1; 61 | complexcmd.data = msg->linear.y; ///< linear y velocity 62 | nbytes = sendto(fd_, &complexcmd, sizeof(complexcmd), 0, 63 | (struct sockaddr *)&addr_qnx_, sizeof(addr_qnx_)); 64 | 65 | complexcmd.cmd_code = 321; 66 | complexcmd.cmd_value = 8; 67 | complexcmd.type = 1; 68 | complexcmd.data = -msg->angular.z; ///< angular velocity 69 | nbytes = sendto(fd_, &complexcmd, sizeof(complexcmd), 0, 70 | (struct sockaddr *)&addr_qnx_, sizeof(addr_qnx_)); 71 | } 72 | 73 | void KickBallCallback(std_msgs::Int32 msg){ 74 | SimpleCMD cmd; 75 | cmd.cmd_code = 503; 76 | cmd.cmd_value = msg.data; 77 | cmd.type = 0; 78 | int nbytes = sendto(fd_, &cmd, sizeof(cmd), 0, 79 | (struct sockaddr *)&addr_qnx_, sizeof(addr_qnx_)); 80 | } 81 | 82 | void SimpleCMDCallback(message_transformer::SimpleCMD msg){ 83 | SimpleCMD cmd; 84 | cmd.cmd_code = msg.cmd_code; 85 | cmd.cmd_value = msg.cmd_value; 86 | cmd.type = msg.type; 87 | int nbytes = sendto(fd_, &cmd, sizeof(cmd), 0, 88 | (struct sockaddr *)&addr_qnx_, sizeof(addr_qnx_)); 89 | } 90 | 91 | void ComplexCMDCallback(message_transformer::ComplexCMD msg){ 92 | ComplexCMD cmd; 93 | cmd.cmd_code = msg.cmd_code; 94 | cmd.cmd_value = msg.cmd_value; 95 | cmd.type = msg.type; 96 | cmd.data = msg.data; 97 | int nbytes = sendto(fd_, &cmd, sizeof(cmd), 0, 98 | (struct sockaddr *)&addr_qnx_, sizeof(addr_qnx_)); 99 | } 100 | 101 | private: 102 | int fd_=-1; 103 | struct sockaddr_in addr_qnx_; 104 | }; 105 | 106 | int main(int argc, char** argv) { 107 | ros::init(argc, argv, "ros2qnx"); 108 | ros::NodeHandle nh; 109 | 110 | ROS2QNX ros2qnx; 111 | ROS_INFO("----- ros2qnx node up -----"); 112 | ros::Subscriber vel_sub = nh.subscribe("cmd_vel", 1, &ROS2QNX::CmdVelCallback, &ros2qnx); 113 | ros::Subscriber vel_sub2 = nh.subscribe("cmd_vel_corrected", 1, &ROS2QNX::CmdVelCallback, &ros2qnx); 114 | ros::Subscriber kickball_sub = nh.subscribe("kick_ball", 1, &ROS2QNX::KickBallCallback, &ros2qnx); 115 | ros::Subscriber simplecmd_sub = nh.subscribe("simple_cmd", 1, &ROS2QNX::SimpleCMDCallback, &ros2qnx); 116 | ros::Subscriber complexcmd_sub = nh.subscribe("complex_cmd", 1, &ROS2QNX::ComplexCMDCallback, &ros2qnx); 117 | 118 | ros::spin(); 119 | 120 | return 0; 121 | } 122 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Jueying Lite3 Perception Development 2 | 3 | [简体中文](./README_ZH.md) 4 | 5 | ## message_transformer 6 | 7 | ### Introduction 8 | 9 | With this package, ROS and UDP messages can be converted to each other. 10 | 11 | The data transmission between the perception host and the motion host or app is based on the UDP protocol. The package ***message_transformer_cpp*** can realize that: 12 | 13 | - Transform UDP messages sent by motion host into ROS topic messages and publish, and send motion control commands issued by perception host to motion host using UDP; 14 | - Receive control commands from the App to turn on and off some functions on perception host. 15 | 16 | 17 | **ROS topics:** 18 | 19 | The package will receive the UDP messages from motion host and publish them to the following topics: 20 | 21 | ```html 22 | Leg Odometry Data: /leg_odom (nav_msgs::Odometry) 23 | IMU Data: /imu/data (sensor_msgs::Imu) 24 | Joint Data: /joint_states (sensor_msgs::JointState) 25 | ``` 26 | 27 | The package will subscribe to the following topics and send the topic messages to motion host. 28 | 29 | ```html 30 | Velocity Command: /cmd_vel (geometry_msgs::Twist) 31 | ``` 32 | 33 | 34 | ### How to Use 35 | 36 | 1. Open a new terminal and enter the following codes to **start the nodes** `nx2app`, `rk2ros`, `ros2rk`: 37 | 38 | ```bash 39 | cd message_transformer_ws/ # go to the package workspace (/home/ysc/message_transformer_ws) 40 | source devel/setup.bash # Add workspace environment variables 41 | roslaunch message_transformer message_transformer.launch # Launch the related nodes 42 | ``` 43 | 44 | 2. Open a new terminal and use `rostopic` command to **check the robot status information**: 45 | 46 | ```bash 47 | rostopic info xxxxxx 48 | rostopic echo xxxxxx # xxxxxx refers to the topic name, and users can subscribe to the topic for secondary development 49 | ``` 50 | 51 | 3. Use the topic `/cmd_vel` to send velocity commands to motion host, in the format of `geometry_msgs/Twist` : 52 | 53 | ```bash 54 | geometry_msgs/Vector3 linear # Linear velocity (m/s) 55 | float64 x # Longitudinal velocity: positive value when going forward 56 | float64 y # Lateral velocity: positive value when going left 57 | float64 z # Invalid parameter 58 | geometry_msgs/Vector3 angular # Angular velocity (rad/s) 59 | float64 x # Invalid parameter 60 | float64 y # Invalid parameter 61 | float64 z # Angular velocity: positive value when turning left 62 | ``` 63 | - Users can publish to this topic in C++ or Python programs compiled based on ROS (refer to http://wiki.ros.org/ROS/Tutorials for learning about ROS ). Users can also publish messages to the topic for debugging in terminal. Please first type the following codes in terminal: 64 | 65 | ```bash 66 | rostopic pub /cmd_vel geometry_msgs/Twist 67 | ``` 68 | 69 | - Add a space after the codes and press Tab key to complement the message type as follows: 70 | 71 | ```bash 72 | rostopic pub /cmd_vel geometry_msgs/Twist "linear: 73 | x: 0.0 74 | y: 0.0 75 | z: 0.0 76 | angular: 77 | x: 0.0 78 | y: 0.0 79 | z: 0.0 80 | " 81 | ``` 82 | - Use the left/right arrow keys on the keyboard to move the cursor, modify the velocity values, and then add ` -r 10` after `geometry_msgs/Twist` to specify the posting frequency (10 times per second) as follows: 83 | 84 | ```bash 85 | rostopic pub /cmd_vel geometry_msgs/Twist -r 10 "linear: 86 | x: 0.2 87 | y: 0.1 88 | z: 0.0 89 | angular: 90 | x: 0.0 91 | y: 0.0 92 | z: 0.3 93 | " 94 | ``` 95 | 96 | - Press Enter key to run it, and the topic messages will be published. 97 | 98 | - This package can subscribe to this topic, transform the topic messages into UDP messages and send them to motion host. 99 | 100 | - After `message_transformer` starts, use the App to switch to auto mode, the robot can act according to the velocity as published. In order to prevent damage to people or objects, please debug in the open space and switch back to the manual mode or press emergency stop in emergency. 101 | 102 | 103 | 104 | ### Package Structure 105 | 106 | ```bash 107 | ~/message_transformer_ws/src/message_transformer 108 | └── message_transformer 109 | ├── CMakeLists.txt 110 | ├── include 111 | │ ├── protocol.h 112 | │ └── sensor_logger_.h 113 | ├── launch 114 | │ └── message_transformer.launch 115 | ├── msg 116 | │ ├── ComplexCMD.msg 117 | │ └── SimpleCMD.msg 118 | ├── package.xml 119 | ├── script 120 | │ ├── log.sh 121 | │ ├── start.sh 122 | │ ├── status.sh 123 | │ ├── stop.sh 124 | └── src 125 | ├── nx2app.cpp 126 | ├── qnx2ros.cpp 127 | ├── ros2qnx.cpp 128 | └── sensor_checker.cpp 129 | ``` 130 | 131 | - ***nx2app.cpp*** is mainly used for UDP communication between perception host and App. App sends command code to perception host and ***nx2app.cpp*** will execute tasks according to the received command. 132 | 133 | - ***qnx2ros.cpp*** is used for receiving the data sent by motion host and transform it into ROS topic messages. 134 | 135 | - ***ros2qnx.cpp*** can subscirbe to the topic published by other nodes, transform the messages into UDP data and send them to motion host. 136 | 137 | - ***sensor_checker.cpp*** is used for checking the status of the sensors on the robot and issuing warnings for malfunctioning sensors. 138 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/include/protocol.h: -------------------------------------------------------------------------------- 1 | #ifndef _PROTOCOL_H_ 2 | #define _PROTOCOL_H_ 3 | 4 | #pragma pack(push, 4) 5 | class SimpleCMD{ 6 | public: 7 | int32_t cmd_code; 8 | int32_t cmd_value; 9 | int32_t type; 10 | }; 11 | 12 | class ComplexCMD : public SimpleCMD{ 13 | public: 14 | double data; 15 | }; 16 | 17 | namespace QNX2ROSProtocol{ 18 | 19 | struct ImuData{ 20 | uint32_t timestamp; 21 | union{ 22 | float buffer_float[9]; 23 | uint8_t buffer_byte[3][12]; 24 | struct{ 25 | float angle_roll,angle_pitch,angle_yaw; 26 | float angular_velocity_roll,angular_velocity_pitch,angular_velocity_yaw; 27 | float acc_x,acc_y,acc_z; 28 | }; 29 | }; 30 | }; 31 | 32 | struct ImuDataReceived { 33 | int code; ///< Command code 34 | int size; ///< Command value 35 | int cons_code; ///< Command type 36 | struct ImuData data; 37 | }; 38 | 39 | /// @brief robot state structe implementation 40 | struct RobotState { 41 | int robot_basic_state; ///< Basic motion state of robot 42 | int robot_gait_state; ///< Robot gait information 43 | double rpy[3]; ///< IMU angular 44 | double rpy_vel[3]; ///< IMU angular velocity 45 | double xyz_acc[3]; ///< IMU acceleration 46 | double pos_world[3]; ///< Position of robot in world coordinate system 47 | double vel_world[3]; ///< The speed of robot in the world coordinate system 48 | double vel_body[3]; ///< Speed of robot in body coordinate system 49 | unsigned touch_down_and_stair_trot; ///< This function has not been activated for the time being. This data is only used to occupy the position 50 | bool is_charging; ///< Not opened temporarily 51 | unsigned error_state; ///< Not opened temporarily 52 | int robot_motion_state; ///< Robot action status 53 | double battery_level; ///< Battery Percentage 54 | int task_state; ///< Not opened temporarily 55 | bool is_robot_need_move; ///< When the robot is standing in place, whether it is pushed to switch into motion mode 56 | bool zero_position_flag; ///< Zero return flag bit 57 | double ultrasound[2]; 58 | }; 59 | struct RobotStateReceived { 60 | int code; ///< Command code 61 | int size; ///< Command value 62 | int cons_code; ///< Command type 63 | struct RobotState data; 64 | }; 65 | 66 | /// @brief Jonint state structe implementation 67 | struct JointState { 68 | double LF_Joint; 69 | double LF_Joint_1; 70 | double LF_Joint_2; 71 | double RF_Joint; 72 | double RF_Joint_1; 73 | double RF_Joint_2; 74 | double LB_Joint; 75 | double LB_Joint_1; 76 | double LB_Joint_2; 77 | double RB_Joint; 78 | double RB_Joint_1; 79 | double RB_Joint_2; 80 | }; 81 | struct JointStateReceived { 82 | int code; 83 | int size; 84 | int cons_code; 85 | struct JointState data; 86 | }; 87 | 88 | /// @brief handlestate state structe implementation 89 | struct HandleState { 90 | double left_axis_forward; ///< Left rocker y-axis, range: - 1~1 91 | double left_axis_side; ///< Left rocker x-axis, range: - 1~1 92 | double right_axis_yaw; ///< right rocker y-axis, range: - 1~1 93 | double goal_vel_forward; ///< Target linear speed in x direction 94 | double goal_vel_side; ///< Target linear speed in y direction 95 | double goal_vel_yaw; ///< Target Yaw angular velocity 96 | }; 97 | struct HandleStateReceived { 98 | int code; 99 | int size; 100 | int cons_code; 101 | struct HandleState data; 102 | }; 103 | 104 | } 105 | #pragma pack(pop) 106 | 107 | 108 | #pragma pack(push,1) 109 | namespace NX2APPProtocol{ 110 | 111 | constexpr uint8_t kHeaderSize = 2; 112 | constexpr uint8_t kChannlSize = 16; 113 | constexpr uint8_t kAxisChannlSize = 4; 114 | constexpr uint8_t kAxisButtonSize = 2; 115 | constexpr uint16_t kJoystickRange = 1000; 116 | constexpr uint16_t kDefaultPort = 43893; 117 | const uint8_t kHeader[kHeaderSize] = {0x55, 0x66}; 118 | namespace ControllerType{ 119 | constexpr uint32_t kRetroid = 1; 120 | } 121 | namespace JoystickKeyStatus{ 122 | constexpr bool kReleased = 0; /**< Key is released. */ 123 | constexpr bool kPressed = 1; /**< Key is pressed. */ 124 | }; 125 | 126 | /// physical botton data 127 | class JoystickHead { 128 | public: 129 | uint8_t stx[2]; /**< Start-of-text bytes for identifying the start of data. */ 130 | uint8_t ctrl; /**< Control byte for additional flags or information. */ 131 | uint16_t data_len; /**< Length of the data in the packet. */ 132 | uint16_t seq; /**< Sequence number for tracking packet order or identifying duplicates. */ 133 | uint8_t id; /**< Identifier for the type of controller. */ 134 | uint16_t checksum; /**< CRC-16 checksum for data integrity verification. */ 135 | }; 136 | 137 | class Channels{ 138 | public: 139 | union { 140 | uint8_t data[kChannlSize * sizeof(uint16_t)]; /**< Array representing raw data channels. */ 141 | struct { 142 | uint16_t buttons[kChannlSize - kAxisChannlSize - kAxisButtonSize]; /**< Array representing button values. */ 143 | int16_t left_axis_x; /**< X-axis value of the left analog stick. */ 144 | int16_t left_axis_y; /**< Y-axis value of the left analog stick. */ 145 | int16_t right_axis_x; /**< X-axis value of the right analog stick. */ 146 | int16_t right_axis_y; /**< Y-axis value of the right analog stick. */ 147 | uint16_t axis_buttons[kAxisButtonSize]; /**< Array representing axis button values. */ 148 | }; 149 | }; 150 | }; 151 | 152 | struct JoystickChannelFrame : public JoystickHead, public Channels 153 | {}; 154 | 155 | struct RetroidKeys { 156 | union { 157 | uint16_t value; /**< Union to represent the keys as a single value. */ 158 | struct { 159 | uint8_t R1 : 1; /**< R1 button. */ 160 | uint8_t L1 : 1; /**< L1 button. */ 161 | uint8_t start : 1; /**< Start button. */ 162 | uint8_t select : 1; /**< Select button. */ 163 | 164 | uint8_t R2 : 1; /**< R2 button. */ 165 | uint8_t L2 : 1; /**< L2 button. */ 166 | 167 | uint8_t A : 1; /**< A button. */ 168 | uint8_t B : 1; /**< B button. */ 169 | uint8_t X : 1; /**< X button. */ 170 | uint8_t Y : 1; /**< Y button. */ 171 | 172 | uint8_t left : 1; /**< Left button on the directional pad. */ 173 | uint8_t right : 1; /**< Right button on the directional pad. */ 174 | uint8_t up : 1; /**< Up button on the directional pad. */ 175 | uint8_t down : 1; /**< Down button on the directional pad. */ 176 | 177 | uint8_t left_axis_button : 1; /**< Left analog stick button. */ 178 | uint8_t right_axis_button : 1; /**< Right analog stick button. */ 179 | }; 180 | }; 181 | union { 182 | float axis_values[4]; /**< Array representing axis values. */ 183 | struct { 184 | float left_axis_x; /**< X-axis value of the left analog stick. */ 185 | float left_axis_y; /**< Y-axis value of the left analog stick. */ 186 | float right_axis_x; /**< X-axis value of the right analog stick. */ 187 | float right_axis_y; /**< Y-axis value of the right analog stick. */ 188 | }; 189 | }; 190 | }; 191 | #pragma pack(pop) 192 | 193 | } 194 | 195 | 196 | 197 | #endif 198 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/src/sensor_checker.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "std_msgs/Float64.h" 8 | #include "std_msgs/Bool.h" 9 | 10 | class Sensor 11 | { 12 | public: 13 | //for check 14 | bool isdebug=false; 15 | double last_time=0; 16 | int silent_limit=5; 17 | int report_times_=0; 18 | std_msgs::Bool isalive; 19 | 20 | //for sub and pub 21 | char name[20]; 22 | ros::Subscriber sub; 23 | ros::Publisher pub; 24 | 25 | Sensor(bool isdebug_) : isdebug(isdebug_) 26 | { 27 | isalive.data=false; 28 | } 29 | 30 | void check() 31 | { 32 | if(last_time == 0) 33 | { 34 | isalive.data = false; 35 | if(report_times_++ < 10){ 36 | ROS_WARN("no [%s] data received since setup", name); 37 | } 38 | } 39 | else if(ros::Time::now().toSec() - last_time > silent_limit) 40 | { 41 | isalive.data = false; 42 | ROS_WARN("[%s] has been silient for: [%f] second." 43 | ,name, ros::Time::now().toSec() - last_time); 44 | } 45 | else 46 | { 47 | isalive.data = true; 48 | } 49 | pub.publish(isalive); 50 | } 51 | }; 52 | 53 | class Imu : public Sensor 54 | { 55 | public: 56 | Imu(ros::NodeHandle n,bool isdebug):Sensor(isdebug) 57 | { 58 | strcpy(name, "imu"); 59 | sub = n.subscribe("/imu/data", 10, &Imu::callback, this); 60 | pub = n.advertise("/sensor_status/imu_isalive", 10); 61 | } 62 | void callback(const sensor_msgs::Imu::ConstPtr& msg) 63 | { 64 | last_time = ros::Time::now().toSec(); 65 | if(isdebug) {ROS_INFO("[%s] refreshed: [%f]", name, last_time);} 66 | } 67 | }; 68 | 69 | class Odom : public Sensor 70 | { 71 | public: 72 | Odom(ros::NodeHandle n,bool isdebug):Sensor(isdebug) 73 | { 74 | strcpy(name, "odom"); 75 | sub = n.subscribe("/leg_odom", 10, &Odom::callback, this); 76 | pub = n.advertise("/sensor_status/odom_isalive", 10); 77 | } 78 | void callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) 79 | { 80 | last_time = ros::Time::now().toSec(); 81 | if(isdebug) {ROS_INFO("[%s] refreshed: [%f]", name, last_time);} 82 | } 83 | }; 84 | 85 | class Odom2 : public Sensor 86 | { 87 | public: 88 | Odom2(ros::NodeHandle n,bool isdebug):Sensor(isdebug) 89 | { 90 | strcpy(name, "odom2"); 91 | sub = n.subscribe("/leg_odom2", 10, &Odom2::callback, this); 92 | pub = n.advertise("/sensor_status/odom2_isalive", 10); 93 | } 94 | void callback(const nav_msgs::Odometry::ConstPtr& msg) 95 | { 96 | last_time = ros::Time::now().toSec(); 97 | if(isdebug) {ROS_INFO("[%s] refreshed: [%f]", name, last_time);} 98 | } 99 | }; 100 | 101 | class Joint : public Sensor 102 | { 103 | public: 104 | Joint(ros::NodeHandle n,bool isdebug):Sensor(isdebug) 105 | { 106 | strcpy(name, "joint"); 107 | sub = n.subscribe("/joint_states", 10, &Joint::callback, this); 108 | pub = n.advertise("/sensor_status/joint_isalive", 10); 109 | 110 | } 111 | void callback(const sensor_msgs::JointState::ConstPtr& msg) 112 | { 113 | last_time = ros::Time::now().toSec(); 114 | if(isdebug) {ROS_INFO("[%s] refreshed: [%f]", name, last_time);} 115 | } 116 | }; 117 | 118 | class Realsense : public Sensor 119 | { 120 | public: 121 | Realsense(ros::NodeHandle n,bool isdebug):Sensor(isdebug) 122 | { 123 | strcpy(name, "realsense"); 124 | sub = n.subscribe("/camera/depth/color/points", 10, &Realsense::callback, this); 125 | pub = n.advertise("/sensor_status/realsense_isalive", 10); 126 | } 127 | void callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 128 | { 129 | last_time = ros::Time::now().toSec(); 130 | if(isdebug) {ROS_INFO("[%s] refreshed: [%f]", name, last_time);} 131 | } 132 | }; 133 | 134 | class Lidar : public Sensor 135 | { 136 | public: 137 | Lidar(ros::NodeHandle n,bool isdebug):Sensor(isdebug) 138 | { 139 | strcpy(name, "lidar"); 140 | sub = n.subscribe("/rslidar_points", 10, &Lidar::callback, this); 141 | pub = n.advertise("/sensor_status/lidar_isalive", 10); 142 | } 143 | void callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 144 | { 145 | last_time = ros::Time::now().toSec(); 146 | if(isdebug) {ROS_INFO("[%s] refreshed: [%f]", name, last_time);} 147 | } 148 | }; 149 | 150 | class Ultrasound : public Sensor 151 | { 152 | public: 153 | Ultrasound(ros::NodeHandle n,bool isdebug):Sensor(isdebug) 154 | { 155 | strcpy(name, "ultrasound"); 156 | sub = n.subscribe("/us_publisher/ultrasound_distance", 10, &Ultrasound::callback, this); 157 | pub = n.advertise("/sensor_status/ultrasound_isalive", 10); 158 | } 159 | void callback(const std_msgs::Float64::ConstPtr& msg) 160 | { 161 | last_time = ros::Time::now().toSec(); 162 | if(isdebug) {ROS_INFO("[%s] refreshed: [%f]", name, last_time);} 163 | } 164 | }; 165 | 166 | class SensorChecker 167 | { 168 | public: 169 | int check_freq_hz; 170 | Imu imu; 171 | Odom odom; 172 | Odom2 odom2; 173 | Joint joint; 174 | Realsense realsense; 175 | Lidar lidar; 176 | Ultrasound ultrasound; 177 | SensorChecker(ros::NodeHandle n, bool isdebug) : 178 | imu(n,isdebug), odom(n,isdebug), odom2(n,isdebug), joint(n,isdebug), 179 | realsense(n,isdebug), lidar(n,isdebug), ultrasound(n,isdebug){}; 180 | 181 | void check(){ 182 | imu.check(); 183 | odom.check(); 184 | odom2.check(); 185 | joint.check(); 186 | realsense.check(); 187 | lidar.check(); 188 | ultrasound.check(); 189 | } 190 | }; 191 | 192 | int main(int argc, char **argv) 193 | { 194 | std::cout << "starting sensor_checker" << std::endl; 195 | //program init 196 | ros::init(argc, argv, "sensor_checker"); 197 | ros::NodeHandle n; 198 | bool isdebug; 199 | bool isdebug_param_ok = n.param("/sensor_checker/isdebug", isdebug, false); 200 | 201 | //SensorChecker init 202 | SensorChecker sensor_checker(n,isdebug); 203 | bool imu_param_ok = n.param("/sensor_checker/imu_silent_limit", sensor_checker.imu.silent_limit, 10); 204 | bool odom_param_ok = n.param("/sensor_checker/odom_silent_limit", sensor_checker.odom.silent_limit, 10); 205 | bool odom2_param_ok = n.param("/sensor_checker/odom2_silent_limit", sensor_checker.odom2.silent_limit, 10); 206 | bool joint_param_ok = n.param("/sensor_checker/joint_silent_limit", sensor_checker.joint.silent_limit, 10); 207 | bool realsense_param_ok = n.param("/sensor_checker/realsense_silent_time", sensor_checker.realsense.silent_limit, 10); 208 | bool lidar_param_ok = n.param("/sensor_checker/lidar_silent_time", sensor_checker.lidar.silent_limit, 10); 209 | bool ultrasound_param_ok = n.param("/sensor_checker/ultrasound_silent_time", sensor_checker.ultrasound.silent_limit, 10); 210 | bool freq_param_ok = n.param("/sensor_checker/check_freq_hz", sensor_checker.check_freq_hz, 10); 211 | if(!imu_param_ok) {ROS_WARN("get param /sensor_checker/imu_silent_limit failed");} 212 | if(!odom_param_ok) {ROS_WARN("get param /sensor_checker/odom_silent_limit failed");} 213 | if(!odom2_param_ok) {ROS_WARN("get param /sensor_checker/odom2_silent_limit failed");} 214 | if(!joint_param_ok) {ROS_WARN("get param /sensor_checker/joint_silent_limit failed");} 215 | if(!realsense_param_ok) {ROS_WARN("get param /sensor_checker/realsense_silent_time failed");} 216 | if(!lidar_param_ok) {ROS_WARN("get param /sensor_checker/lidar_silent_time failed");} 217 | if(!ultrasound_param_ok) {ROS_WARN("get param /sensor_checker/ultrasound_silent_time failed");} 218 | if(!freq_param_ok) {ROS_WARN("get param /sensor_checker/check_freq_hz failed");} 219 | if(!isdebug_param_ok) {ROS_WARN("get param /sensor_checker/isdebug failed");} 220 | 221 | //loop 222 | ros::Rate loop_rate(sensor_checker.check_freq_hz); 223 | while (ros::ok()) 224 | { 225 | if(isdebug) {ROS_INFO("time now is: [%f]",ros::Time::now().toSec());} 226 | sensor_checker.check(); 227 | ros::spinOnce(); 228 | loop_rate.sleep(); 229 | } 230 | return 0; 231 | } 232 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/src/qnx2ros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "tf/transform_datatypes.h" 24 | #include 25 | #include "../include/protocol.h" 26 | 27 | using namespace std; 28 | #define PI 3.1415926 29 | 30 | class QNX2ROS{ 31 | public: 32 | QNX2ROS(ros::NodeHandle nh, int server_port){ 33 | socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0); 34 | if (socket_fd_ < 0){ 35 | perror("socket"); 36 | exit(1); 37 | } 38 | 39 | memset(&listen_addr_, 0, sizeof(struct sockaddr_in)); ///< initialize to zeros 40 | listen_addr_.sin_family = AF_INET; ///< host byte order 41 | listen_addr_.sin_port = htons(server_port); ///< port in network byte order 42 | listen_addr_.sin_addr.s_addr = htonl(INADDR_ANY); ///< automatically fill in my IP 43 | if (bind(socket_fd_, (struct sockaddr *)&listen_addr_, sizeof(listen_addr_)) < 0) 44 | { 45 | perror("bind error:"); 46 | exit(1); 47 | } 48 | 49 | leg_odom_pub_ = nh.advertise("leg_odom", 1); 50 | leg_odom_pub2_ = nh.advertise("leg_odom2", 1); 51 | joint_state_pub_ = nh.advertise("joint_states", 1); 52 | // imu_pub_100_ = nh.advertise("/imu/data", 1); 53 | imu_pub_200_ = nh.advertise("/imu/data", 1); 54 | handle_pub_ = nh.advertise("/handle_state", 1); 55 | ultrasound_pub_ = nh.advertise("/us_publisher/ultrasound_distance", 1); 56 | } 57 | 58 | ~QNX2ROS(){ 59 | close(socket_fd_); 60 | } 61 | 62 | void ReceiveFrame(){ 63 | int len = sizeof(listen_addr_); 64 | recv_num_ = recvfrom(socket_fd_, receive_buffer_, sizeof(receive_buffer_), 0, (struct sockaddr *)&listen_addr_, (socklen_t *)&len); 65 | if(recv_num_<0){ 66 | perror("recvfrom error:"); 67 | exit(1); 68 | } 69 | 70 | times_++; 71 | if(times_==3000) 72 | { 73 | ROS_INFO("qnx2ros received: [%d] bytes",recv_num_); 74 | times_=0; 75 | } 76 | } 77 | 78 | void ParseFrame(){ 79 | switch(recv_num_) 80 | { 81 | case sizeof(QNX2ROSProtocol::RobotStateReceived): 82 | ParseRobotStateFrame(); 83 | break; 84 | 85 | case sizeof(QNX2ROSProtocol::JointStateReceived): 86 | ParseJointStateFrame(); 87 | break; 88 | 89 | case sizeof(QNX2ROSProtocol::HandleStateReceived): 90 | ParseHandleStateFrame(); 91 | break; 92 | 93 | case sizeof(QNX2ROSProtocol::ImuDataReceived): 94 | ParseImuDataFrame(); 95 | break; 96 | 97 | default: 98 | // ROS_WARN("udp pack length not fit into any"); 99 | break; 100 | } 101 | } 102 | 103 | void ParseRobotStateFrame(){ 104 | QNX2ROSProtocol::RobotStateReceived *dr = (QNX2ROSProtocol::RobotStateReceived *)(receive_buffer_); 105 | QNX2ROSProtocol::RobotState *robot_state = &dr->data; 106 | 107 | if (dr->code == 2305) 108 | { 109 | geometry_msgs::PoseWithCovarianceStamped leg_odom_data; 110 | leg_odom_data.header.frame_id = "odom"; 111 | leg_odom_data.header.stamp = ros::Time::now(); 112 | leg_odom_data.pose.pose.orientation = 113 | tf::createQuaternionMsgFromYaw(robot_state->rpy[2] / 180 * PI); 114 | leg_odom_data.pose.pose.position.x = robot_state->pos_world[0]; 115 | leg_odom_data.pose.pose.position.y = robot_state->pos_world[1]; 116 | leg_odom_data.pose.pose.position.z = robot_state->pos_world[2]; 117 | leg_odom_pub_.publish(leg_odom_data); 118 | 119 | nav_msgs::Odometry leg_odom_data2; 120 | leg_odom_data2.header.stamp = ros::Time::now(); 121 | leg_odom_data2.pose = leg_odom_data.pose; 122 | leg_odom_data2.twist.twist.linear.x = robot_state->vel_body[0]; 123 | leg_odom_data2.twist.twist.linear.y = robot_state->vel_body[1]; 124 | leg_odom_data2.twist.twist.angular.z = robot_state->rpy_vel[2]; 125 | leg_odom_pub2_.publish(leg_odom_data2); 126 | 127 | sensor_msgs::Imu imu_msg; 128 | imu_msg.header.frame_id = "imu"; 129 | imu_msg.header.stamp = ros::Time::now(); 130 | auto q = tf::createQuaternionFromRPY(robot_state->rpy[0] / 180 * PI, 131 | robot_state->rpy[1] / 180 * PI, 132 | robot_state->rpy[2] / 180 * PI); 133 | tf::quaternionTFToMsg(q, imu_msg.orientation); 134 | imu_msg.angular_velocity.x = robot_state->rpy_vel[0]; 135 | imu_msg.angular_velocity.y = robot_state->rpy_vel[1]; 136 | imu_msg.angular_velocity.z = robot_state->rpy_vel[2]; 137 | imu_msg.linear_acceleration.x = robot_state->xyz_acc[0]; 138 | imu_msg.linear_acceleration.y = robot_state->xyz_acc[1]; 139 | imu_msg.linear_acceleration.z = robot_state->xyz_acc[2]; 140 | // imu_pub_100_.publish(imu_msg); 141 | 142 | std_msgs::Float64 ultrasound_distance; 143 | ultrasound_distance.data = robot_state->ultrasound[1]; 144 | ultrasound_pub_.publish(ultrasound_distance); 145 | 146 | geometry_msgs::TransformStamped leg_odom_trans; 147 | leg_odom_trans.header.stamp = ros::Time::now(); 148 | leg_odom_trans.header.frame_id = "odom"; 149 | leg_odom_trans.child_frame_id = "base_link"; 150 | leg_odom_trans.transform.translation.x = leg_odom_data.pose.pose.position.x; 151 | leg_odom_trans.transform.translation.y = leg_odom_data.pose.pose.position.y; 152 | leg_odom_trans.transform.translation.z = leg_odom_data.pose.pose.position.z; 153 | leg_odom_trans.transform.rotation = imu_msg.orientation; 154 | // odom_broadcaster_.sendTransform(leg_odom_trans); 155 | } 156 | } 157 | 158 | void ParseJointStateFrame(){ 159 | QNX2ROSProtocol::JointStateReceived *dr = (QNX2ROSProtocol::JointStateReceived *)(receive_buffer_); 160 | QNX2ROSProtocol::JointState *joint_state = &dr->data; 161 | if (dr->code == 2306) 162 | { 163 | sensor_msgs::JointState joint_state_data; 164 | joint_state_data.header.stamp = ros::Time::now(); 165 | joint_state_data.name.resize(12); 166 | joint_state_data.position.resize(12); 167 | 168 | joint_state_data.name[0] = "LF_Joint"; 169 | joint_state_data.position[0] = -joint_state->LF_Joint; 170 | joint_state_data.name[1] = "LF_Joint_1"; 171 | joint_state_data.position[1] = -joint_state->LF_Joint_1; 172 | joint_state_data.name[2] = "LF_Joint_2"; 173 | joint_state_data.position[2] = -joint_state->LF_Joint_2; 174 | 175 | joint_state_data.name[3] = "RF_Joint"; 176 | joint_state_data.position[3] = -joint_state->RF_Joint; 177 | joint_state_data.name[4] = "RF_Joint_1"; 178 | joint_state_data.position[4] = -joint_state->RF_Joint_1; 179 | joint_state_data.name[5] = "RF_Joint_2"; 180 | joint_state_data.position[5] = -joint_state->RF_Joint_2; 181 | 182 | joint_state_data.name[6] = "LB_Joint"; 183 | joint_state_data.position[6] = -joint_state->LB_Joint; 184 | joint_state_data.name[7] = "LB_Joint_1"; 185 | joint_state_data.position[7] = -joint_state->LB_Joint_1; 186 | joint_state_data.name[8] = "LB_Joint_2"; 187 | joint_state_data.position[8] = -joint_state->LB_Joint_2; 188 | 189 | joint_state_data.name[9] = "RB_Joint"; 190 | joint_state_data.position[9] = -joint_state->RB_Joint; 191 | joint_state_data.name[10] = "RB_Joint_1"; 192 | joint_state_data.position[10] = -joint_state->RB_Joint_1; 193 | joint_state_data.name[11] = "RB_Joint_2"; 194 | joint_state_data.position[11] = -joint_state->RB_Joint_2; 195 | joint_state_pub_.publish(joint_state_data); 196 | } 197 | } 198 | 199 | void ParseHandleStateFrame(){ 200 | QNX2ROSProtocol::HandleStateReceived *dr = (QNX2ROSProtocol::HandleStateReceived *)(receive_buffer_); 201 | QNX2ROSProtocol::HandleState *handle_state = &dr->data; 202 | if (dr->code == 2309) 203 | { 204 | geometry_msgs::Twist handle_state_msg; 205 | handle_state_msg.linear.x = handle_state->left_axis_forward; 206 | handle_state_msg.linear.y = handle_state->left_axis_side; 207 | handle_state_msg.angular.z = - handle_state->right_axis_yaw; 208 | handle_pub_.publish(handle_state_msg); 209 | } 210 | } 211 | 212 | void ParseImuDataFrame(){ 213 | QNX2ROSProtocol::ImuDataReceived *dr = (QNX2ROSProtocol::ImuDataReceived *)(receive_buffer_); 214 | QNX2ROSProtocol::ImuData *imu_data = &dr->data; 215 | 216 | if(dr->code == 0x010901) 217 | { 218 | sensor_msgs::Imu imu_msg; 219 | imu_msg.header.frame_id = "base_link"; 220 | imu_msg.header.stamp = ros::Time::now(); 221 | auto q = tf::createQuaternionFromRPY(imu_data->angle_roll / 180 * PI, 222 | imu_data->angle_pitch / 180 * PI, 223 | imu_data->angle_yaw / 180 * PI); 224 | tf::quaternionTFToMsg(q, imu_msg.orientation); 225 | imu_msg.angular_velocity.x = imu_data->angular_velocity_roll; 226 | imu_msg.angular_velocity.y = imu_data->angular_velocity_pitch; 227 | imu_msg.angular_velocity.z = imu_data->angular_velocity_yaw; 228 | imu_msg.linear_acceleration.x = imu_data->acc_x; 229 | imu_msg.linear_acceleration.y = imu_data->acc_y; 230 | imu_msg.linear_acceleration.z = imu_data->acc_z; 231 | imu_pub_200_.publish(imu_msg); 232 | } 233 | } 234 | 235 | void RunRoutine(){ 236 | ReceiveFrame(); 237 | ParseFrame(); 238 | } 239 | 240 | private: 241 | int socket_fd_=-1; 242 | struct sockaddr_in listen_addr_; 243 | int recv_num_ = -1; 244 | uint8_t receive_buffer_[512]; 245 | int times_=0; 246 | 247 | ros::Publisher leg_odom_pub_; 248 | ros::Publisher leg_odom_pub2_; 249 | ros::Publisher joint_state_pub_; 250 | // ros::Publisher imu_pub_100_; 251 | ros::Publisher imu_pub_200_; 252 | ros::Publisher handle_pub_; 253 | ros::Publisher ultrasound_pub_; 254 | tf::TransformBroadcaster odom_broadcaster_; 255 | }; 256 | 257 | int main(int argc, char **argv) { 258 | ros::init(argc, argv, "qnx2ros"); 259 | ros::NodeHandle nh; 260 | int server_port; 261 | nh.param("server_port", server_port, 43897); 262 | QNX2ROS qnx2ros(nh, server_port); 263 | ros::Rate loop_rate(10000); 264 | while (ros::ok()) 265 | { 266 | ros::spinOnce(); 267 | qnx2ros.RunRoutine(); 268 | loop_rate.sleep(); 269 | } 270 | return 0; 271 | } 272 | -------------------------------------------------------------------------------- /message_transformer/message_transformer/src/nx2app.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "std_msgs/Int32.h" 25 | #include "std_msgs/Bool.h" 26 | #include "std_msgs/Float64.h" 27 | #include 28 | #include "tf/transform_datatypes.h" 29 | #include 30 | #include 31 | #include "../include/protocol.h" 32 | #include "sensor_msgs/Joy.h" 33 | #include "../include/sensor_logger.h" 34 | #include 35 | 36 | using namespace std; 37 | 38 | class FunctionState 39 | { 40 | public: 41 | bool obstacle_avoidance = false; 42 | //TODO: 43 | bool people_tracking = false; 44 | bool slam = false; 45 | bool navigation = false; 46 | }; 47 | 48 | using namespace NX2APPProtocol; 49 | 50 | class NX2APP 51 | { 52 | public: 53 | NX2APP(ros::NodeHandle nh, int server_port){ 54 | sensors_logger_ = std::make_unique(nh); 55 | 56 | socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0); 57 | 58 | memset(&listen_addr_, 0, sizeof(struct sockaddr_in)); 59 | listen_addr_.sin_family = AF_INET; 60 | listen_addr_.sin_port = htons(server_port); 61 | listen_addr_.sin_addr.s_addr = htonl(INADDR_ANY); 62 | bind(socket_fd_, (struct sockaddr *)&listen_addr_, sizeof(listen_addr_)) < 0; 63 | 64 | logfile_.open("/home/ysc/lite_cog/transfer/nx2app_log.txt"); 65 | logfile_ << "start logging" << endl; 66 | 67 | joy_pub_ = nh.advertise("commands/joy_raw", 1); 68 | } 69 | 70 | ~NX2APP(){ 71 | close(socket_fd_); 72 | logfile_.close(); 73 | } 74 | 75 | void ReceiveFrame(){ 76 | int len = sizeof(listen_addr_); 77 | if((recv_num_ = recvfrom(socket_fd_, receive_buffer_, sizeof(receive_buffer_), 0,(struct sockaddr *)&send_addr_,(socklen_t *)&len)) < 0) 78 | { 79 | perror("recvfrom error:"); 80 | exit(1); 81 | } 82 | } 83 | 84 | void LogNewRound(){ 85 | logfile_ << "-----new round-----" << endl; 86 | if(!sensors_logger_->imu_->isalive_){ 87 | logfile_ << "imu is down" << endl; 88 | } 89 | if(!sensors_logger_->odom_->isalive_){ 90 | logfile_ << "odom is down" << endl; 91 | } 92 | if(!sensors_logger_->odom2_->isalive_){ 93 | logfile_ << "odom2 is down" << endl; 94 | } 95 | if(!sensors_logger_->joint_->isalive_){ 96 | logfile_ << "joint is down" << endl; 97 | } 98 | if(!sensors_logger_->realsense_->isalive_){ 99 | logfile_ << "realsense is down" << endl; 100 | } 101 | if(!sensors_logger_->lidar_->isalive_){ 102 | logfile_ << "lidar is down" << endl; 103 | } 104 | if(!sensors_logger_->ultrasound_->isalive_){ 105 | logfile_ << "ultrasound is down" << endl; 106 | } 107 | } 108 | 109 | void ParseFrame(){ 110 | switch (recv_num_) 111 | { 112 | case sizeof(SimpleCMD): 113 | ParseSimpleCMDFrame(); 114 | break; 115 | 116 | case sizeof(JoystickChannelFrame): 117 | ParseJoystickChannelFrame(); 118 | break; 119 | 120 | default: 121 | break; 122 | } 123 | } 124 | 125 | void ParseSimpleCMDFrame(){ 126 | SimpleCMD *dr = (SimpleCMD *)(receive_buffer_); 127 | switch (dr->cmd_code) 128 | { 129 | case 0x21012109:{ 130 | //start obstacle avoidance 131 | if (dr->cmd_value==0x40 && function_state_.obstacle_avoidance == false) 132 | { 133 | if(sensors_logger_->imu_->isalive_) 134 | { 135 | logfile_ << "start obstacle avoidance" << endl; 136 | int ret; 137 | ret = system("systemctl start realsense.service &"); 138 | logfile_ << "systemctl start realsense.service & " << ret << endl; 139 | ret = system("systemctl start voa.service &"); 140 | logfile_ << "systemctl start realsense.service & " << ret << endl; 141 | function_state_.obstacle_avoidance = true; 142 | } 143 | else 144 | { 145 | logfile_ << "imu not ready, obstacle avoidance not started." << endl; 146 | } 147 | } 148 | //Turn off all ai functions 149 | if (dr->cmd_value==0x00) 150 | { 151 | logfile_ << "kill all functions" << endl; 152 | int ret; 153 | ret = system("systemctl stop realsense.service &"); 154 | logfile_ << "systemctl stop realsense.service & " << ret << endl; 155 | ret = system("systemctl stop voa.service &"); 156 | logfile_ << "systemctl stop voa.service & " << ret << endl; 157 | function_state_.obstacle_avoidance = false; 158 | } 159 | break; 160 | } 161 | //Inquire obstacle avoidance state 162 | case 0x2101210D:{ 163 | char outBuf[7]=""; 164 | FILE *fp = popen("systemctl is-active voa.service", "r"); 165 | if(fp){ 166 | char *ret = fgets(outBuf, 7, fp); 167 | pclose(fp); 168 | } 169 | SimpleCMD message; 170 | if(memcmp(outBuf,"active",6)==0) 171 | { 172 | logfile_ << "voa active" << endl; 173 | message.cmd_code = 0x2101210D; 174 | message.cmd_value = 0x11; 175 | message.type = 0; 176 | ssize_t nbytes = sendto(socket_fd_, &message, sizeof(message), 0, 177 | (struct sockaddr *)&send_addr_, sizeof(send_addr_)); 178 | logfile_ << "bytes send" << nbytes << endl; 179 | logfile_ << "ip: " << inet_ntoa(send_addr_.sin_addr) << endl; 180 | logfile_ << "port: " << ntohs(send_addr_.sin_port) << endl; 181 | } 182 | else 183 | { 184 | logfile_ << "voa not active" << endl; 185 | message.cmd_code = 0x2101210D; 186 | message.cmd_value = 0x10; 187 | message.type = 0; 188 | ssize_t nbytes = sendto(socket_fd_, &message, sizeof(message), 0, 189 | (struct sockaddr *)&send_addr_, sizeof(send_addr_)); 190 | logfile_ << "bytes send" << nbytes << endl; 191 | logfile_ << "ip: " << inet_ntoa(send_addr_.sin_addr) << endl; 192 | logfile_ << "port: " << ntohs(send_addr_.sin_port) << endl; 193 | } 194 | break; 195 | } 196 | 197 | default: 198 | break; 199 | } 200 | } 201 | 202 | void ParseJoystickChannelFrame(){ 203 | ROS_WARN("ParseJoystickChannelFrame"); 204 | JoystickChannelFrame *frame = (JoystickChannelFrame *)(receive_buffer_); 205 | if(frame->stx[0] != kHeader[0] || frame->stx[1] != kHeader[1]){ 206 | ROS_WARN("Receiving a JoystickChannelFrame with incorrect stx!"); 207 | return; 208 | } 209 | if (frame->id != (uint8_t)ControllerType::kRetroid){ 210 | ROS_WARN("Receiving a JoystickChannelFrame with incorrect id!"); 211 | return; 212 | } 213 | uint16_t checksum=0; 214 | uint8_t *pdata=frame->data; 215 | for(int i=0; idata_len; i++){ 216 | checksum+=static_cast(pdata[i]); 217 | } 218 | if(checksum!=frame->checksum){ 219 | ROS_WARN("Receiving a JoystickChannelFrame with incorrect checksum!"); 220 | return; 221 | } 222 | 223 | std::bitset value_bit(0); 224 | int16_t ch[kChannlSize]; 225 | memcpy(ch, frame->data, sizeof(ch)); 226 | for(int i = 0; i < kChannlSize; i++){ 227 | value_bit[i] = ch[i]; 228 | } 229 | joystick_state_.value = value_bit.to_ulong(); 230 | 231 | joystick_state_.left = (frame->left_axis_x == -kJoystickRange) ? (uint8_t)JoystickKeyStatus::kPressed 232 | : ((frame->left_axis_x == kJoystickRange) ? (uint8_t)JoystickKeyStatus::kReleased : (uint8_t)JoystickKeyStatus::kReleased); 233 | joystick_state_.right = (frame->left_axis_x == kJoystickRange) ? (uint8_t)JoystickKeyStatus::kPressed 234 | : ((frame->left_axis_x == -kJoystickRange) ? (uint8_t)JoystickKeyStatus::kReleased : (uint8_t)JoystickKeyStatus::kReleased); 235 | joystick_state_.up = (frame->left_axis_y == kJoystickRange) ? (uint8_t)JoystickKeyStatus::kPressed 236 | : ((frame->left_axis_y == -kJoystickRange) ? (uint8_t)JoystickKeyStatus::kReleased : (uint8_t)JoystickKeyStatus::kReleased); 237 | joystick_state_.down = (frame->left_axis_y == -kJoystickRange) ? (uint8_t)JoystickKeyStatus::kPressed 238 | : ((frame->left_axis_y == kJoystickRange) ? (uint8_t)JoystickKeyStatus::kReleased : (uint8_t)JoystickKeyStatus::kReleased); 239 | 240 | joystick_state_.left_axis_x = frame->left_axis_x/(float)kJoystickRange; 241 | joystick_state_.left_axis_y = frame->left_axis_y/(float)kJoystickRange; 242 | joystick_state_.right_axis_x = frame->right_axis_x/(float)kJoystickRange; 243 | joystick_state_.right_axis_y = frame->right_axis_y/(float)kJoystickRange; 244 | 245 | sensor_msgs::Joy msg; 246 | msg.header.stamp = ros::Time::now(); 247 | msg.header.frame_id = "joy"; 248 | msg.axes.push_back(-joystick_state_.left_axis_x); 249 | msg.axes.push_back(joystick_state_.left_axis_y); 250 | if(joystick_state_.L2){ 251 | msg.axes.push_back(-1); 252 | } 253 | else{ 254 | msg.axes.push_back(1); 255 | } 256 | msg.axes.push_back(-joystick_state_.right_axis_x); 257 | msg.axes.push_back(joystick_state_.right_axis_y); 258 | if(joystick_state_.R2){ 259 | msg.axes.push_back(-1); 260 | } 261 | else{ 262 | msg.axes.push_back(1); 263 | } 264 | if(joystick_state_.left){ 265 | msg.axes.push_back(1); 266 | } 267 | else if(joystick_state_.right){ 268 | msg.axes.push_back(-1); 269 | } 270 | else{ 271 | msg.axes.push_back(0); 272 | } 273 | if(joystick_state_.up){ 274 | msg.axes.push_back(1); 275 | } 276 | else if(joystick_state_.down){ 277 | msg.axes.push_back(-1); 278 | } 279 | else{ 280 | msg.axes.push_back(0); 281 | } 282 | msg.buttons.push_back(joystick_state_.A); 283 | msg.buttons.push_back(joystick_state_.B); 284 | msg.buttons.push_back(joystick_state_.X); 285 | msg.buttons.push_back(joystick_state_.Y); 286 | msg.buttons.push_back(joystick_state_.L1); 287 | msg.buttons.push_back(joystick_state_.R1); 288 | msg.buttons.push_back(joystick_state_.select); 289 | msg.buttons.push_back(joystick_state_.select); 290 | msg.buttons.push_back(joystick_state_.start); 291 | msg.buttons.push_back(joystick_state_.left_axis_button); 292 | msg.buttons.push_back(joystick_state_.right_axis_button); 293 | joy_pub_.publish(msg); 294 | } 295 | 296 | void RunRoutine(){ 297 | ReceiveFrame(); 298 | ros::spinOnce(); 299 | LogNewRound(); 300 | ParseFrame(); 301 | } 302 | 303 | private: 304 | int socket_fd_=-1; 305 | struct sockaddr_in listen_addr_; 306 | struct sockaddr_in send_addr_; 307 | 308 | int recv_num_; 309 | uint8_t receive_buffer_[512]; 310 | ofstream logfile_; 311 | 312 | FunctionState function_state_; 313 | RetroidKeys joystick_state_; 314 | 315 | ros::Publisher joy_pub_; 316 | 317 | std::unique_ptr sensors_logger_; 318 | }; 319 | 320 | int main(int argc, char **argv) { 321 | ros::init(argc, argv, "nx2app"); 322 | ros::NodeHandle nh; 323 | int server_port; 324 | nh.param("server_port", server_port, 43899); 325 | NX2APP nx2app(nh, server_port); 326 | ros::Rate loop_rate(100); 327 | ROS_INFO("nx2app node start"); 328 | while(ros::ok()){ 329 | nx2app.RunRoutine(); 330 | loop_rate.sleep(); 331 | } 332 | return 0; 333 | } 334 | --------------------------------------------------------------------------------