updated template to support scout/scout mini/scout mini omni with v1 and v2 protocol

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 

scout_base/include/scout_base/scout_messenger.hpp

@@ -15,11 +15,11 @@
 
 #include <ros/ros.h>
 #include <nav_msgs/Odometry.h>
-// #include <tf/transform_broadcaster.h>
 #include <tf2_ros/transform_broadcaster.h>
 
 #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<std::mutex> 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 <typename T,
+            std::enable_if_t<!std::is_base_of<ScoutMiniOmniRobot, T>::value,
+                             bool> = true>
+  void SetScoutMotionCommand(std::shared_ptr<T> base,
+                             const geometry_msgs::Twist::ConstPtr &msg) {
+    base->SetMotionCommand(msg->linear.x, msg->angular.z);
+  }
+
+  template <typename T,
+            std::enable_if_t<std::is_base_of<ScoutMiniOmniRobot, T>::value,
+                             bool> = true>
+  void SetScoutMotionCommand(std::shared_ptr<T> 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;
+    double linear_speed = msg.linear_velocity;
-    angular_speed_ = angular;
+    double angular_speed = msg.angular_velocity;
+    double lateral_speed = 0;
 
-    double d_x = linear_speed_ * std::cos(theta_) * dt;
+    if (std::is_base_of<ScoutMiniOmniRobot, ScoutType>::value) {
-    double d_y = linear_speed_ * std::sin(theta_) * dt;
+      lateral_speed = msg.lateral_velocity;
-    double d_theta = angular_speed_ * dt;
+    } 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.x = linear_speed;
-    odom_msg.twist.twist.linear.y = 0.0;
+    odom_msg.twist.twist.linear.y = lateral_speed;
-    odom_msg.twist.twist.angular.z = angular_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,
+    PublishOdometryToROS(state.motion_state, dt);
-                         state.motion_state.angular_velocity, 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
 

scout_base/launch/scout_base.launch

@@ -11,11 +11,13 @@
     -->
     <arg name="port_name" default="can0" />
     <arg name="is_scout_mini" default="false" />
+    <arg name="is_omni_wheel" default="false" />
     <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" />

scout_base/launch/scout_mini_base.launch

@@ -11,11 +11,13 @@
     -->
     <arg name="port_name" default="can0" />
     <arg name="is_scout_mini" default="true" />
+    <arg name="is_omni_wheel" default="false" />
     <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" />

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<std::string>("base_frame", base_frame,
                                   std::string("base_link"));
+  private_node.param<bool>("is_omni_wheel", is_omni_wheel, false);
   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,
@@ -46,21 +46,21 @@ int main(int argc, char **argv) {
     ROS_INFO("Robot base: Scout");
   }
 
-  std::shared_ptr<ScoutRobot> robot;
+  if (!is_omni_wheel) {
-  std::unique_ptr<ScoutMessenger<ScoutRobot>> messenger;
-
     // instantiate a robot object
+    std::shared_ptr<ScoutRobot> robot;
+
     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 =
+        robot = std::make_shared<ScoutRobot>(ProtocolVersion::AGX_V1,
-          std::make_shared<ScoutRobot>(ProtocolVersion::AGX_V1, is_scout_mini);
+                                             is_scout_mini);
       } else if (proto == ProtocolVersion::AGX_V2) {
         std::cout << "Detected protocol: AGX_V2" << std::endl;
-      robot =
+        robot = std::make_shared<ScoutRobot>(ProtocolVersion::AGX_V2,
-          std::make_shared<ScoutRobot>(ProtocolVersion::AGX_V2, is_scout_mini);
+                                             is_scout_mini);
       } else {
         std::cout << "Detected protocol: UNKONWN" << std::endl;
         return -1;
@@ -70,7 +70,8 @@ int main(int argc, char **argv) {
     }
 
     // instantiate a ROS messenger
-  messenger = std::unique_ptr<ScoutMessenger<ScoutRobot>>(
+    std::unique_ptr<ScoutMessenger<ScoutRobot>> messenger =
+        std::unique_ptr<ScoutMessenger<ScoutRobot>>(
             new ScoutMessenger<ScoutRobot>(robot, &node));
 
     messenger->SetOdometryFrame(odom_frame);
@@ -90,6 +91,50 @@ int main(int argc, char **argv) {
     }
 
     messenger->Run();
+  } else {
+    // instantiate a robot object
+    std::shared_ptr<ScoutMiniOmniRobot> 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::unique_ptr<ScoutMiniOmniRobot>(
+          new ScoutMiniOmniRobot(ProtocolVersion::AGX_V1));
+
+    } else if (proto == ProtocolVersion::AGX_V2) {
+      std::cout << "Detected protocol: AGX_V2" << std::endl;
+      robot = std::unique_ptr<ScoutMiniOmniRobot>(
+          new ScoutMiniOmniRobot(ProtocolVersion::AGX_V2));
+    } else {
+      std::cout << "Detected protocol: UNKONWN" << std::endl;
+      return -1;
+    }
+
+    // instantiate a ROS messenger
+    std::unique_ptr<ScoutMessenger<ScoutMiniOmniRobot>> messenger =
+        std::unique_ptr<ScoutMessenger<ScoutMiniOmniRobot>>(
+            new ScoutMessenger<ScoutMiniOmniRobot>(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();
+  }
 
   return 0;
 }