From 3cdcda0e6f5990f9ff008f72656a289bab6b4714 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 1 Apr 2022 16:22:18 +0900 Subject: [PATCH] [unitree_legged_real] add rosparam to ros_server_udp --- unitree_legged_real/src/exe/ros_server_udp.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/unitree_legged_real/src/exe/ros_server_udp.cpp b/unitree_legged_real/src/exe/ros_server_udp.cpp index 6d3ee4f..2349837 100644 --- a/unitree_legged_real/src/exe/ros_server_udp.cpp +++ b/unitree_legged_real/src/exe/ros_server_udp.cpp @@ -18,7 +18,7 @@ using namespace UNITREE_LEGGED_SDK; class Ros_Server_High { public: - Ros_Server_High(): udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState)){ + Ros_Server_High(uint16_t local_port, const char* target_ip, uint16_t target_port): udp(local_port, target_ip, target_port, 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); @@ -123,7 +123,14 @@ void Ros_Server_High::RobotControl() int main(int argc, char *argv[]){ ros::init(argc, argv, "ros_server"); - Ros_Server_High server; + int local_port = 8090; + std::string target_ip = std::string("192.168.123.161"); + int target_port = 8082; + ros::param::get("~local_port", local_port); + ros::param::get("~target_ip", target_ip); + ros::param::get("~target_port", target_port); + + Ros_Server_High server(local_port, target_ip.c_str(), target_port); 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));