From 886beaca33aec04fbfc63c43207b553704df7eb8 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 29 Mar 2022 18:44:30 +0900 Subject: [PATCH] add CMakeLists.txt src/exe/ros_server_udp.cpp --- unitree_legged_real/CMakeLists.txt | 4 + .../src/exe/ros_server_udp.cpp | 107 ++++++++++++++++++ 2 files changed, 111 insertions(+) create mode 100644 unitree_legged_real/src/exe/ros_server_udp.cpp diff --git a/unitree_legged_real/CMakeLists.txt b/unitree_legged_real/CMakeLists.txt index 26902f3..908b8a1 100755 --- a/unitree_legged_real/CMakeLists.txt +++ b/unitree_legged_real/CMakeLists.txt @@ -38,6 +38,10 @@ add_executable(lcm_server ${ROBOT_SDK}/unitree_legged_sdk/examples/lcm_server.cp target_link_libraries(lcm_server ${EXTRA_LIBS} ${catkin_LIBRARIES}) add_dependencies(lcm_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_executable(ros_server src/exe/ros_server_udp.cpp) +target_link_libraries(ros_server ${EXTRA_LIBS} ${catkin_LIBRARIES}) +add_dependencies(ros_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + add_executable(position_lcm src/exe/position_mode.cpp) target_link_libraries(position_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES}) add_dependencies(position_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/unitree_legged_real/src/exe/ros_server_udp.cpp b/unitree_legged_real/src/exe/ros_server_udp.cpp new file mode 100644 index 0000000..3ffa1e3 --- /dev/null +++ b/unitree_legged_real/src/exe/ros_server_udp.cpp @@ -0,0 +1,107 @@ +/************************************************************************ +Copyright (c) 2020, Kei Okada. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "convert.h" + +using namespace UNITREE_LEGGED_SDK; +// High command Lcm Server +class Ros_Server_High +{ +public: + Ros_Server_High(): udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState)){ + pub = n.advertise("high_state", 1); + sub = n.subscribe("high_cmd", 1, [&](const unitree_legged_msgs::HighCmdConstPtr& msg){SendHighROS=*msg;}); + sub_vel = n.subscribe("cmd_vel", 1, &Ros_Server_High::CmdVelCB, this); + + udp.InitCmdData(cmd); + } + void CmdVelCB(const geometry_msgs::TwistConstPtr& msg){ + SendHighROS.mode = 2; // 2. target velocity walking (controlled by velocity + yawSpeed) + SendHighROS.velocity[0] = msg->linear.x; + SendHighROS.velocity[1] = msg->linear.y; + SendHighROS.yawSpeed = msg->angular.z; + } + void UDPRecv(){ + udp.Recv(); + } + void UDPSend(){ + udp.Send(); + } + void RobotControl(); + + UDP udp; + HighCmd cmd = {0}; + HighState state = {0}; + + ros::NodeHandle n; + ros::Publisher pub; + ros::Subscriber sub, sub_vel; + unitree_legged_msgs::HighCmd SendHighROS; + unitree_legged_msgs::HighState RecvHighROS; +}; + + +void Ros_Server_High::RobotControl() +{ + udp.GetRecv(state); + RecvHighROS = ToRos(state); + pub.publish(RecvHighROS); + + // need to keep original cmd.head to move robots + std::array head = cmd.head; + cmd = ToLcm(SendHighROS, cmd); + cmd.head = head; + + // + ROS_INFO_THROTTLE(1, "head %8d %8d", cmd.head[0], cmd.head[1]); + ROS_INFO_THROTTLE(1, "levelFlag %8d", cmd.levelFlag); + ROS_INFO_THROTTLE(1, "frameReserve %8d", cmd.frameReserve); + ROS_INFO_THROTTLE(1, "SN %8d %8d", cmd.SN[0], cmd.SN[1]); + ROS_INFO_THROTTLE(1, "version %8d %8d", cmd.version[0], cmd.version[1]); + ROS_INFO_THROTTLE(1, "bandWidth %8d", cmd.bandWidth); + ROS_INFO_THROTTLE(1, "mode %8d", cmd.mode); + ROS_INFO_THROTTLE(1, "gaitType %d", cmd.gaitType); + ROS_INFO_THROTTLE(1, "speedLevel %d", cmd.speedLevel); + ROS_INFO_THROTTLE(1, "footRaiseHeight %f", cmd.footRaiseHeight); + ROS_INFO_THROTTLE(1, "bodyHeight %f", cmd.bodyHeight); + ROS_INFO_THROTTLE(1, "position %f %f", cmd.postion[0], cmd.postion[1]); + ROS_INFO_THROTTLE(1, "euler %f %f %f", cmd.euler[0], cmd.euler[1], cmd.euler[2]); + ROS_INFO_THROTTLE(1, "velocity %f %f", cmd.velocity[0], cmd.velocity[1]); + ROS_INFO_THROTTLE(1, "yawSpeed %f", cmd.yawSpeed); + ROS_INFO_THROTTLE(1, "bms %d", cmd.bms.off); + ROS_INFO_THROTTLE(1, "led (%d %d %d)", cmd.led[0].r, cmd.led[0].g, cmd.led[0].b); + ROS_INFO_THROTTLE(1, " (%d %d %d)", cmd.led[1].r, cmd.led[1].g, cmd.led[1].b); + ROS_INFO_THROTTLE(1, " (%d %d %d)", cmd.led[2].r, cmd.led[2].g, cmd.led[2].b); + ROS_INFO_THROTTLE(1, " (%d %d %d)", cmd.led[3].r, cmd.led[3].g, cmd.led[3].b); + //ROS_INFO_THROTTLE(1, "wirelessRemote ");for(int i = 0; i < 40; i++) ROS_INFO_THROTTLE(1, " %d", cmd.wirelessRemote[i]); ROS_INFO_THROTTLE(1, ""); + ROS_INFO_THROTTLE(1, "reserve %d", cmd.reserve); + ROS_INFO_THROTTLE(1, "crc %d", cmd.crc); + + // + udp.SetSend(cmd); +} + +int main(int argc, char *argv[]){ + ros::init(argc, argv, "ros_server"); + + Ros_Server_High server; + LoopFunc loop_control("control_loop", 0.002, boost::bind(&Ros_Server_High::RobotControl, &server)); + LoopFunc loop_udpSend("UDP_Send", 0.002, 3, boost::bind(&Ros_Server_High::UDPSend, &server)); + LoopFunc loop_udpRecv("UDP_Recv", 0.002, 3, boost::bind(&Ros_Server_High::UDPRecv, &server)); + + loop_udpSend.start(); + loop_udpRecv.start(); + loop_control.start(); + + ros::spin(); +}