From 266d68d5fe3d76da19ebd2241c1862a8c95414c9 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Tue, 5 Oct 2021 19:01:10 +0800 Subject: [PATCH] updated template to support scout/scout mini/scout mini omni with v1 and v2 protocol --- scout_base/CMakeLists.txt | 2 +- .../include/scout_base/scout_messenger.hpp | 103 ++++++----------- scout_base/launch/scout_base.launch | 2 + scout_base/launch/scout_mini_base.launch | 2 + scout_base/launch/scout_mini_omni_base.launch | 2 +- scout_base/src/scout_base_node.cpp | 107 +++++++++++++----- 6 files changed, 117 insertions(+), 101 deletions(-) diff --git a/scout_base/CMakeLists.txt b/scout_base/CMakeLists.txt index bb171a4..7693fb8 100644 --- a/scout_base/CMakeLists.txt +++ b/scout_base/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(scout_base) ## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) +add_compile_options(-std=c++14) find_package(catkin REQUIRED COMPONENTS roslaunch diff --git a/scout_base/include/scout_base/scout_messenger.hpp b/scout_base/include/scout_base/scout_messenger.hpp index 88c3e2b..f6f67ab 100644 --- a/scout_base/include/scout_base/scout_messenger.hpp +++ b/scout_base/include/scout_base/scout_messenger.hpp @@ -15,11 +15,11 @@ #include #include -// #include #include #include "scout_msgs/ScoutStatus.h" #include "scout_msgs/ScoutLightCmd.h" + #include "ugv_sdk/mobile_robot/scout_robot.hpp" namespace westonrobot { @@ -71,9 +71,6 @@ class ScoutMessenger { tf2_ros::TransformBroadcaster tf_broadcaster_; // speed variables - double linear_speed_ = 0.0; - double angular_speed_ = 0.0; - double position_x_ = 0.0; double position_y_ = 0.0; double theta_ = 0.0; @@ -95,7 +92,7 @@ class ScoutMessenger { void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) { if (!simulated_robot_) { - scout_->SetMotionCommand(msg->linear.x, msg->angular.z); + SetScoutMotionCommand(scout_, msg); } else { std::lock_guard guard(twist_mutex_); current_twist_ = *msg.get(); @@ -103,6 +100,22 @@ class ScoutMessenger { ROS_INFO("Cmd received:%f, %f", msg->linear.x, msg->angular.z); } + template ::value, + bool> = true> + void SetScoutMotionCommand(std::shared_ptr base, + const geometry_msgs::Twist::ConstPtr &msg) { + base->SetMotionCommand(msg->linear.x, msg->angular.z); + } + + template ::value, + bool> = true> + void SetScoutMotionCommand(std::shared_ptr base, + const geometry_msgs::Twist::ConstPtr &msg) { + base->SetMotionCommand(msg->linear.x, msg->angular.z, msg->linear.y); + } + void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg) { if (!simulated_robot_) { // if (msg->cmd_ctrl_allowed) { @@ -162,14 +175,21 @@ class ScoutMessenger { } } - void PublishOdometryToROS(double linear, double angular, double dt) { + void PublishOdometryToROS(const MotionStateMessage &msg, double dt) { // perform numerical integration to get an estimation of pose - linear_speed_ = linear; - angular_speed_ = angular; + double linear_speed = msg.linear_velocity; + double angular_speed = msg.angular_velocity; + double lateral_speed = 0; - double d_x = linear_speed_ * std::cos(theta_) * dt; - double d_y = linear_speed_ * std::sin(theta_) * dt; - double d_theta = angular_speed_ * dt; + if (std::is_base_of::value) { + lateral_speed = msg.lateral_velocity; + } else { + lateral_speed = 0; + } + + double d_x = linear_speed * std::cos(theta_) * dt; + double d_y = (linear_speed * std::sin(theta_) + lateral_speed) * dt; + double d_theta = angular_speed * dt; position_x_ += d_x; position_y_ += d_y; @@ -202,9 +222,9 @@ class ScoutMessenger { odom_msg.pose.pose.position.z = 0.0; odom_msg.pose.pose.orientation = odom_quat; - odom_msg.twist.twist.linear.x = linear_speed_; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.angular.z = angular_speed_; + odom_msg.twist.twist.linear.x = linear_speed; + odom_msg.twist.twist.linear.y = lateral_speed; + odom_msg.twist.twist.angular.z = angular_speed; odom_pub_.publish(odom_msg); } @@ -271,64 +291,11 @@ class ScoutMessenger { status_pub_.publish(status_msg); // publish odometry and tf - PublishOdometryToROS(state.motion_state.linear_velocity, - state.motion_state.angular_velocity, dt); + PublishOdometryToROS(state.motion_state, dt); // record time for next integration last_time_ = current_time_; } - - void PublishSimStateToROS(double linear, double angular) { - current_time_ = ros::Time::now(); - - double dt = (current_time_ - last_time_).toSec(); - - static bool init_run = true; - if (init_run) { - last_time_ = current_time_; - init_run = false; - return; - } - - // publish scout state message - scout_msgs::ScoutStatus status_msg; - - status_msg.header.stamp = current_time_; - - status_msg.linear_velocity = linear; - status_msg.angular_velocity = angular; - - status_msg.vehicle_state = 0x00; - status_msg.control_mode = 0x01; - status_msg.error_code = 0x00; - status_msg.battery_voltage = 29.5; - - // for (int i = 0; i < 4; ++i) - // { - // status_msg.motor_states[i].current = state.motor_states[i].current; - // status_msg.motor_states[i].rpm = state.motor_states[i].rpm; - // status_msg.motor_states[i].temperature = - // state.motor_states[i].temperature; - // } - - status_msg.light_control_enabled = false; - // status_msg.front_light_state.mode = state.front_light_state.mode; - // status_msg.front_light_state.custom_value = - // state.front_light_state.custom_value; status_msg.rear_light_state.mode = - // state.rear_light_state.mode; status_msg.rear_light_state.custom_value = - // state.front_light_state.custom_value; - - status_pub_.publish(status_msg); - - // publish odometry and tf - PublishOdometryToROS(linear, angular, dt); - - // record time for next integration - last_time_ = current_time_; - } - - // void PublishSimStateToROS(double linear, double angular); - // void GetCurrentMotionCmdForSim(double &linear, double &angular); }; } // namespace westonrobot diff --git a/scout_base/launch/scout_base.launch b/scout_base/launch/scout_base.launch index 9575120..cbb3704 100644 --- a/scout_base/launch/scout_base.launch +++ b/scout_base/launch/scout_base.launch @@ -11,11 +11,13 @@ --> + + diff --git a/scout_base/launch/scout_mini_base.launch b/scout_base/launch/scout_mini_base.launch index 76fa2d7..28f4da7 100644 --- a/scout_base/launch/scout_mini_base.launch +++ b/scout_base/launch/scout_mini_base.launch @@ -11,11 +11,13 @@ --> + + diff --git a/scout_base/launch/scout_mini_omni_base.launch b/scout_base/launch/scout_mini_omni_base.launch index c97895b..e3c1362 100644 --- a/scout_base/launch/scout_mini_omni_base.launch +++ b/scout_base/launch/scout_mini_omni_base.launch @@ -11,7 +11,7 @@ --> - + diff --git a/scout_base/src/scout_base_node.cpp b/scout_base/src/scout_base_node.cpp index 14d1eaa..e89d1d8 100644 --- a/scout_base/src/scout_base_node.cpp +++ b/scout_base/src/scout_base_node.cpp @@ -18,13 +18,12 @@ int main(int argc, char **argv) { ros::init(argc, argv, "scout_node"); ros::NodeHandle node(""), private_node("~"); - // std::signal(SIGINT, DetachRobot); - // fetch parameters before connecting to robot std::string port_name; std::string odom_frame; std::string base_frame; std::string odom_topic_name; + bool is_omni_wheel = false; bool is_simulated = false; int sim_rate = 50; bool is_scout_mini = false; @@ -34,6 +33,7 @@ int main(int argc, char **argv) { std::string("odom")); private_node.param("base_frame", base_frame, std::string("base_link")); + private_node.param("is_omni_wheel", is_omni_wheel, false); private_node.param("simulated_robot", is_simulated, false); private_node.param("control_rate", sim_rate, 50); private_node.param("odom_topic_name", odom_topic_name, @@ -46,50 +46,95 @@ int main(int argc, char **argv) { ROS_INFO("Robot base: Scout"); } - std::shared_ptr robot; - std::unique_ptr> messenger; + if (!is_omni_wheel) { + // instantiate a robot object + std::shared_ptr robot; - // instantiate a robot object - ProtocolDectctor detector; - if (detector.Connect(port_name)) { + ProtocolDectctor detector; + if (detector.Connect(port_name)) { + auto proto = detector.DetectProtocolVersion(5); + if (proto == ProtocolVersion::AGX_V1) { + std::cout << "Detected protocol: AGX_V1" << std::endl; + robot = std::make_shared(ProtocolVersion::AGX_V1, + is_scout_mini); + } else if (proto == ProtocolVersion::AGX_V2) { + std::cout << "Detected protocol: AGX_V2" << std::endl; + robot = std::make_shared(ProtocolVersion::AGX_V2, + is_scout_mini); + } else { + std::cout << "Detected protocol: UNKONWN" << std::endl; + return -1; + } + } else { + return -1; + } + + // instantiate a ROS messenger + std::unique_ptr> messenger = + std::unique_ptr>( + new ScoutMessenger(robot, &node)); + + messenger->SetOdometryFrame(odom_frame); + messenger->SetBaseFrame(base_frame); + messenger->SetOdometryTopicName(odom_topic_name); + if (is_simulated) messenger->SetSimulationMode(sim_rate); + + // connect to robot and setup ROS subscription + if (port_name.find("can") != std::string::npos) { + if (robot->Connect(port_name)) { + robot->EnableCommandedMode(); + ROS_INFO("Using CAN bus to talk with the robot"); + } else { + ROS_INFO("Failed to connect to the robot CAN bus"); + return -1; + } + } + + messenger->Run(); + } else { + // instantiate a robot object + std::shared_ptr robot; + + ProtocolDectctor detector; + detector.Connect(port_name); auto proto = detector.DetectProtocolVersion(5); if (proto == ProtocolVersion::AGX_V1) { std::cout << "Detected protocol: AGX_V1" << std::endl; - robot = - std::make_shared(ProtocolVersion::AGX_V1, is_scout_mini); + robot = std::unique_ptr( + new ScoutMiniOmniRobot(ProtocolVersion::AGX_V1)); + } else if (proto == ProtocolVersion::AGX_V2) { std::cout << "Detected protocol: AGX_V2" << std::endl; - robot = - std::make_shared(ProtocolVersion::AGX_V2, is_scout_mini); + robot = std::unique_ptr( + new ScoutMiniOmniRobot(ProtocolVersion::AGX_V2)); } else { std::cout << "Detected protocol: UNKONWN" << std::endl; return -1; } - } else { - return -1; - } - // instantiate a ROS messenger - messenger = std::unique_ptr>( - new ScoutMessenger(robot, &node)); + // instantiate a ROS messenger + std::unique_ptr> messenger = + std::unique_ptr>( + new ScoutMessenger(robot, &node)); - messenger->SetOdometryFrame(odom_frame); - messenger->SetBaseFrame(base_frame); - messenger->SetOdometryTopicName(odom_topic_name); - if (is_simulated) messenger->SetSimulationMode(sim_rate); + messenger->SetOdometryFrame(odom_frame); + messenger->SetBaseFrame(base_frame); + messenger->SetOdometryTopicName(odom_topic_name); + if (is_simulated) messenger->SetSimulationMode(sim_rate); - // connect to robot and setup ROS subscription - if (port_name.find("can") != std::string::npos) { - if (robot->Connect(port_name)) { - robot->EnableCommandedMode(); - ROS_INFO("Using CAN bus to talk with the robot"); - } else { - ROS_INFO("Failed to connect to the robot CAN bus"); - return -1; + // connect to robot and setup ROS subscription + if (port_name.find("can") != std::string::npos) { + if (robot->Connect(port_name)) { + robot->EnableCommandedMode(); + ROS_INFO("Using CAN bus to talk with the robot"); + } else { + ROS_INFO("Failed to connect to the robot CAN bus"); + return -1; + } } - } - messenger->Run(); + messenger->Run(); + } return 0; } \ No newline at end of file