From 7d9f027137783cb750f77d96f4012b1a990f6ad6 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Tue, 5 Oct 2021 16:54:36 +0800 Subject: [PATCH] messenger: cleaned up messenger interface --- .../include/scout_base/scout_messenger.hpp | 45 +++++---- .../include/scout_base/scout_params.hpp | 33 ------- scout_base/launch/scout_mini_omni_base.launch | 28 ++++++ scout_base/src/scout_base_node.cpp | 95 ++++++++++++------- scout_base/src/scout_messenger.cpp | 81 +++++++++++----- 5 files changed, 172 insertions(+), 110 deletions(-) delete mode 100644 scout_base/include/scout_base/scout_params.hpp create mode 100644 scout_base/launch/scout_mini_omni_base.launch diff --git a/scout_base/include/scout_base/scout_messenger.hpp b/scout_base/include/scout_base/scout_messenger.hpp index 497ab87..e00470c 100644 --- a/scout_base/include/scout_base/scout_messenger.hpp +++ b/scout_base/include/scout_base/scout_messenger.hpp @@ -22,10 +22,22 @@ #include "ugv_sdk/mobile_robot/scout_robot.hpp" namespace westonrobot { -class ScoutROSMessenger { +class ScoutMessenger { public: - explicit ScoutROSMessenger(ros::NodeHandle *nh); - ScoutROSMessenger(std::shared_ptr scout, ros::NodeHandle *nh); + explicit ScoutMessenger(ros::NodeHandle *nh); + ScoutMessenger(std::shared_ptr scout, ros::NodeHandle *nh); + + void SetOdometryFrame(std::string frame); + void SetBaseFrame(std::string frame); + void SetOdometryTopicName(std::string name); + void SetSimulationMode(int loop_rate); + + void Run(); + void Stop(); + + private: + std::shared_ptr scout_; + ros::NodeHandle *nh_; std::string odom_frame_; std::string base_frame_; @@ -34,29 +46,20 @@ class ScoutROSMessenger { bool simulated_robot_ = false; int sim_control_rate_ = 50; - void SetupSubscription(); - - void PublishStateToROS(); - void PublishSimStateToROS(double linear, double angular); - - void GetCurrentMotionCmdForSim(double &linear, double &angular); - - private: - std::shared_ptr scout_; - ros::NodeHandle *nh_; - + std::atomic keep_running_; std::mutex twist_mutex_; geometry_msgs::Twist current_twist_; - ros::Publisher odom_publisher_; - ros::Publisher status_publisher_; - ros::Subscriber motion_cmd_subscriber_; - ros::Subscriber light_cmd_subscriber_; + ros::Publisher odom_pub_; + ros::Publisher status_pub_; + ros::Subscriber motion_cmd_sub_; + ros::Subscriber light_cmd_sub_; 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; @@ -64,9 +67,15 @@ class ScoutROSMessenger { ros::Time last_time_; ros::Time current_time_; + void SetupSubscription(); + void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg); void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg); void PublishOdometryToROS(double linear, double angular, double dt); + + void PublishStateToROS(); + void PublishSimStateToROS(double linear, double angular); + void GetCurrentMotionCmdForSim(double &linear, double &angular); }; } // namespace westonrobot diff --git a/scout_base/include/scout_base/scout_params.hpp b/scout_base/include/scout_base/scout_params.hpp deleted file mode 100644 index 0165362..0000000 --- a/scout_base/include/scout_base/scout_params.hpp +++ /dev/null @@ -1,33 +0,0 @@ -/* - * scout_params.hpp - * - * Created on: Sep 27, 2019 15:08 - * Description: - * - * Copyright (c) 2020 Ruixiang Du (rdu) - */ - -#ifndef SCOUT_PARAMS_HPP -#define SCOUT_PARAMS_HPP - -namespace westonrobot { -struct ScoutParams { - /* Scout Parameters */ - static constexpr double max_steer_angle = 30.0; // in degree - - static constexpr double track = - 0.58306; // in meter (left & right wheel distance) - static constexpr double wheelbase = - 0.498; // in meter (front & rear wheel distance) - static constexpr double wheel_radius = 0.165; // in meter - - // from user manual v1.2.8 P18 - // max linear velocity: 1.5 m/s - // max angular velocity: 0.7853 rad/s - static constexpr double max_linear_speed = 1.5; // in m/s - static constexpr double max_angular_speed = 0.7853; // in rad/s - static constexpr double max_speed_cmd = 10.0; // in rad/s -}; -} // namespace westonrobot - -#endif /* SCOUT_PARAMS_HPP */ diff --git a/scout_base/launch/scout_mini_omni_base.launch b/scout_base/launch/scout_mini_omni_base.launch new file mode 100644 index 0000000..c97895b --- /dev/null +++ b/scout_base/launch/scout_mini_omni_base.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + diff --git a/scout_base/src/scout_base_node.cpp b/scout_base/src/scout_base_node.cpp index 84710af..0e68872 100644 --- a/scout_base/src/scout_base_node.cpp +++ b/scout_base/src/scout_base_node.cpp @@ -7,14 +7,17 @@ #include #include "ugv_sdk/mobile_robot/scout_robot.hpp" +#include "ugv_sdk/utilities/protocol_detector.hpp" + #include "scout_base/scout_messenger.hpp" using namespace westonrobot; std::shared_ptr robot; +std::unique_ptr messenger; bool keep_run = true; -void DetachRobot(int signal) { keep_run = false; } +void DetachRobot(int signal) { messenger->Stop(); } int main(int argc, char **argv) { // setup ROS node @@ -23,46 +26,72 @@ int main(int argc, char **argv) { std::signal(SIGINT, DetachRobot); - // instantiate a robot object - robot = std::make_shared(); - ScoutROSMessenger messenger(robot, &node); - // fetch parameters before connecting to robot std::string port_name; - private_node.param("port_name", port_name, std::string("can0")); - private_node.param("odom_frame", messenger.odom_frame_, - std::string("odom")); - private_node.param("base_frame", messenger.base_frame_, - std::string("base_link")); - private_node.param("simulated_robot", messenger.simulated_robot_, - false); - private_node.param("control_rate", messenger.sim_control_rate_, 50); - private_node.param("odom_topic_name", messenger.odom_topic_name_, - std::string("odom")); + std::string odom_frame; + std::string base_frame; + std::string odom_topic_name; + bool is_simulated = false; + int sim_rate = 50; + bool is_scout_mini = false; - if (!messenger.simulated_robot_) { - // connect to robot and setup ROS subscription - if (port_name.find("can") != std::string::npos) { - robot->Connect(port_name); + private_node.param("port_name", port_name, std::string("can0")); + private_node.param("odom_frame", odom_frame, + std::string("odom")); + private_node.param("base_frame", base_frame, + std::string("base_link")); + 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, + std::string("odom")); + private_node.param("is_scout_mini", is_scout_mini, false); + + if (is_scout_mini) { + ROS_INFO("Robot base: Scout Mini"); + } else { + ROS_INFO("Robot base: Scout"); + } + + // instantiate a robot object + 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 + 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"); - } - } - messenger.SetupSubscription(); - - // publish robot state at 50Hz while listening to twist commands - ros::Rate rate(50); - while (keep_run) { - if (!messenger.simulated_robot_) { - messenger.PublishStateToROS(); } else { - double linear, angular; - messenger.GetCurrentMotionCmdForSim(linear, angular); - messenger.PublishSimStateToROS(linear, angular); + ROS_INFO("Failed to connect to the robot CAN bus"); + return -1; } - ros::spinOnce(); - rate.sleep(); } + messenger->Run(); + return 0; } \ No newline at end of file diff --git a/scout_base/src/scout_messenger.cpp b/scout_base/src/scout_messenger.cpp index 204d463..fcecaba 100644 --- a/scout_base/src/scout_messenger.cpp +++ b/scout_base/src/scout_messenger.cpp @@ -14,27 +14,56 @@ #include "scout_msgs/ScoutStatus.h" namespace westonrobot { -ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) +ScoutMessenger::ScoutMessenger(ros::NodeHandle *nh) : scout_(nullptr), nh_(nh) {} -ScoutROSMessenger::ScoutROSMessenger(std::shared_ptr scout, - ros::NodeHandle *nh) +ScoutMessenger::ScoutMessenger(std::shared_ptr scout, + ros::NodeHandle *nh) : scout_(scout), nh_(nh) {} -void ScoutROSMessenger::SetupSubscription() { - // odometry publisher - odom_publisher_ = nh_->advertise(odom_topic_name_, 50); - status_publisher_ = - nh_->advertise("/scout_status", 10); - - // cmd subscriber - motion_cmd_subscriber_ = nh_->subscribe( - "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); - light_cmd_subscriber_ = nh_->subscribe( - "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); +void ScoutMessenger::SetOdometryFrame(std::string frame) { + odom_frame_ = frame; } -void ScoutROSMessenger::TwistCmdCallback( +void ScoutMessenger::SetBaseFrame(std::string frame) { base_frame_ = frame; } + +void ScoutMessenger::SetOdometryTopicName(std::string name) { + odom_topic_name_ = name; +} + +void ScoutMessenger::SetSimulationMode(int loop_rate) { + simulated_robot_ = true; + sim_control_rate_ = loop_rate; +} + +void ScoutMessenger::SetupSubscription() { + // odometry publisher + odom_pub_ = nh_->advertise(odom_topic_name_, 50); + status_pub_ = nh_->advertise("/scout_status", 10); + + // cmd subscriber + motion_cmd_sub_ = nh_->subscribe( + "/cmd_vel", 5, &ScoutMessenger::TwistCmdCallback, this); + light_cmd_sub_ = nh_->subscribe( + "/scout_light_control", 5, &ScoutMessenger::LightCmdCallback, this); +} + +void ScoutMessenger::Run() { + SetupSubscription(); + + // publish robot state at 50Hz while listening to twist commands + keep_running_ = true; + ros::Rate rate(50); + while (keep_running_) { + PublishStateToROS(); + ros::spinOnce(); + rate.sleep(); + } +} + +void ScoutMessenger::Stop() { keep_running_ = false; } + +void ScoutMessenger::TwistCmdCallback( const geometry_msgs::Twist::ConstPtr &msg) { if (!simulated_robot_) { scout_->SetMotionCommand(msg->linear.x, msg->angular.z); @@ -42,17 +71,17 @@ void ScoutROSMessenger::TwistCmdCallback( std::lock_guard guard(twist_mutex_); current_twist_ = *msg.get(); } - // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z); + ROS_INFO("Cmd received:%f, %f", msg->linear.x, msg->angular.z); } -void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, - double &angular) { +void ScoutMessenger::GetCurrentMotionCmdForSim(double &linear, + double &angular) { std::lock_guard guard(twist_mutex_); linear = current_twist_.linear.x; angular = current_twist_.angular.z; } -void ScoutROSMessenger::LightCmdCallback( +void ScoutMessenger::LightCmdCallback( const scout_msgs::ScoutLightCmd::ConstPtr &msg) { if (!simulated_robot_) { // if (msg->cmd_ctrl_allowed) { @@ -112,7 +141,7 @@ void ScoutROSMessenger::LightCmdCallback( } } -void ScoutROSMessenger::PublishStateToROS() { +void ScoutMessenger::PublishStateToROS() { current_time_ = ros::Time::now(); double dt = (current_time_ - last_time_).toSec(); @@ -171,7 +200,7 @@ void ScoutROSMessenger::PublishStateToROS() { status_msg.rear_light_state.mode = state.light_state.rear_light.mode; status_msg.rear_light_state.custom_value = state.light_state.rear_light.custom_value; - status_publisher_.publish(status_msg); + status_pub_.publish(status_msg); // publish odometry and tf PublishOdometryToROS(state.motion_state.linear_velocity, @@ -181,7 +210,7 @@ void ScoutROSMessenger::PublishStateToROS() { last_time_ = current_time_; } -void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) { +void ScoutMessenger::PublishSimStateToROS(double linear, double angular) { current_time_ = ros::Time::now(); double dt = (current_time_ - last_time_).toSec(); @@ -221,7 +250,7 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) { // state.rear_light_state.mode; status_msg.rear_light_state.custom_value = // state.front_light_state.custom_value; - status_publisher_.publish(status_msg); + status_pub_.publish(status_msg); // publish odometry and tf PublishOdometryToROS(linear, angular, dt); @@ -230,8 +259,8 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) { last_time_ = current_time_; } -void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, - double dt) { +void ScoutMessenger::PublishOdometryToROS(double linear, double angular, + double dt) { // perform numerical integration to get an estimation of pose linear_speed_ = linear; angular_speed_ = angular; @@ -274,6 +303,6 @@ void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, odom_msg.twist.twist.linear.y = 0.0; odom_msg.twist.twist.angular.z = angular_speed_; - odom_publisher_.publish(odom_msg); + odom_pub_.publish(odom_msg); } } // namespace westonrobot \ No newline at end of file