messenger: cleaned up messenger interface

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);
+  explicit ScoutMessenger(ros::NodeHandle *nh);
-  ScoutROSMessenger(std::shared_ptr<ScoutRobot> scout, ros::NodeHandle *nh);
+  ScoutMessenger(std::shared_ptr<ScoutRobot> 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<ScoutRobot> 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();
+  std::atomic<bool> keep_running_;
-
-  void PublishStateToROS();
-  void PublishSimStateToROS(double linear, double angular);
-
-  void GetCurrentMotionCmdForSim(double &linear, double &angular);
-
- private:
-  std::shared_ptr<ScoutRobot> scout_;
-  ros::NodeHandle *nh_;
-
   std::mutex twist_mutex_;
   geometry_msgs::Twist current_twist_;
 
-  ros::Publisher odom_publisher_;
+  ros::Publisher odom_pub_;
-  ros::Publisher status_publisher_;
+  ros::Publisher status_pub_;
-  ros::Subscriber motion_cmd_subscriber_;
+  ros::Subscriber motion_cmd_sub_;
-  ros::Subscriber light_cmd_subscriber_;
+  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
 

scout_base/include/scout_base/scout_params.hpp

@@ -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 */

scout_base/launch/scout_mini_omni_base.launch

@@ -0,0 +1,28 @@
+<launch>
+    <!-- 
+        The robot can be controlled either through CAN bus or UART port. Make sure the hardware
+        interface is set up correctly before attempting to connect to the robot.
+        
+        You only need to specify the port name, such as "can0", "/dev/ttyUSB0". The port should 
+        operate with the following configuration:
+        
+        * CAN bus: 500k
+        * UART: 115200, Parity None, 8-bit Data, 1 Stop Bit 
+    -->
+    <arg name="port_name" default="can0" />
+    <arg name="is_scout_mini" default="true" />
+    <arg name="is_omni_wheel" default="true" />
+    <arg name="simulated_robot" default="false" />
+    <arg name="odom_topic_name" default="odom" />
+
+    <node name="scout_base_node" pkg="scout_base" type="scout_base_node" output="screen">
+        <param name="is_scout_mini" type="bool" value="$(arg is_scout_mini)" />
+        <param name="is_omni_wheel" type="bool" value="$(arg is_omni_wheel)" />
+        <param name="port_name" type="string" value="$(arg port_name)" />
+        <param name="simulated_robot" type="bool" value="$(arg simulated_robot)" />
+        <param name="odom_frame" type="string" value="odom" />
+        <param name="base_frame" type="string" value="base_link" />
+        <param name="odom_topic_name" type="string" value="$(arg odom_topic_name)" />
+    </node>
+
+</launch>

scout_base/src/scout_base_node.cpp

@@ -7,14 +7,17 @@
 #include <tf/transform_broadcaster.h>
 
 #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<ScoutRobot> robot;
+std::unique_ptr<ScoutMessenger> 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<ScoutRobot>();
-  ScoutROSMessenger messenger(robot, &node);
-
   // fetch parameters before connecting to robot
   std::string port_name;
-  private_node.param<std::string>("port_name", port_name, std::string("can0"));
+  std::string odom_frame;
-  private_node.param<std::string>("odom_frame", messenger.odom_frame_,
+  std::string base_frame;
-                                  std::string("odom"));
+  std::string odom_topic_name;
-  private_node.param<std::string>("base_frame", messenger.base_frame_,
+  bool is_simulated = false;
-                                  std::string("base_link"));
+  int sim_rate = 50;
-  private_node.param<bool>("simulated_robot", messenger.simulated_robot_,
+  bool is_scout_mini = false;
-                           false);
-  private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);
-  private_node.param<std::string>("odom_topic_name", messenger.odom_topic_name_,
-                                  std::string("odom"));
 
-  if (!messenger.simulated_robot_) {
+  private_node.param<std::string>("port_name", port_name, std::string("can0"));
-    // connect to robot and setup ROS subscription
+  private_node.param<std::string>("odom_frame", odom_frame,
-    if (port_name.find("can") != std::string::npos) {
+                                  std::string("odom"));
-      robot->Connect(port_name);
+  private_node.param<std::string>("base_frame", base_frame,
+                                  std::string("base_link"));
+  private_node.param<bool>("simulated_robot", is_simulated, false);
+  private_node.param<int>("control_rate", sim_rate, 50);
+  private_node.param<std::string>("odom_topic_name", odom_topic_name,
+                                  std::string("odom"));
+  private_node.param<bool>("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<ScoutRobot>(ProtocolVersion::AGX_V1, is_scout_mini);
+    } else if (proto == ProtocolVersion::AGX_V2) {
+      std::cout << "Detected protocol: AGX_V2" << std::endl;
+      robot =
+          std::make_shared<ScoutRobot>(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<ScoutMessenger>(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;
+      ROS_INFO("Failed to connect to the robot CAN bus");
-      messenger.GetCurrentMotionCmdForSim(linear, angular);
+      return -1;
-      messenger.PublishSimStateToROS(linear, angular);
     }
-    ros::spinOnce();
-    rate.sleep();
   }
 
+  messenger->Run();
+
   return 0;
 }

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<ScoutRobot> scout,
+ScoutMessenger::ScoutMessenger(std::shared_ptr<ScoutRobot> scout,
-                                     ros::NodeHandle *nh)
+                               ros::NodeHandle *nh)
     : scout_(scout), nh_(nh) {}
 
-void ScoutROSMessenger::SetupSubscription() {
+void ScoutMessenger::SetOdometryFrame(std::string frame) {
-  // odometry publisher
+  odom_frame_ = frame;
-  odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
-  status_publisher_ =
-      nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
-
-  // cmd subscriber
-  motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>(
-      "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this);
-  light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
-      "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
 }
 
-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<nav_msgs::Odometry>(odom_topic_name_, 50);
+  status_pub_ = nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
+
+  // cmd subscriber
+  motion_cmd_sub_ = nh_->subscribe<geometry_msgs::Twist>(
+      "/cmd_vel", 5, &ScoutMessenger::TwistCmdCallback, this);
+  light_cmd_sub_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
+      "/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<std::mutex> 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,
+void ScoutMessenger::GetCurrentMotionCmdForSim(double &linear,
-                                                  double &angular) {
+                                               double &angular) {
   std::lock_guard<std::mutex> 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,
+void ScoutMessenger::PublishOdometryToROS(double linear, double angular,
-                                             double dt) {
+                                          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