From 5c695eccfcbe7853c7fd6c6dbe7aa016708ca653 Mon Sep 17 00:00:00 2001
From: Ruixiang Du <ruixiang.du@westonrobot.com>
Date: Mon, 21 Dec 2020 11:43:30 +0800
Subject: [PATCH] updated scout mini support

---
 scout_base/launch/scout_base.launch |  2 ++
 scout_base/src/scout_base_node.cpp  | 22 +++++++++++++++-------
 2 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/scout_base/launch/scout_base.launch b/scout_base/launch/scout_base.launch
index dd5bb55..9575120 100644
--- a/scout_base/launch/scout_base.launch
+++ b/scout_base/launch/scout_base.launch
@@ -10,10 +10,12 @@
         * UART: 115200, Parity None, 8-bit Data, 1 Stop Bit 
     -->
     <arg name="port_name" default="can0" />
+    <arg name="is_scout_mini" 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="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" />
diff --git a/scout_base/src/scout_base_node.cpp b/scout_base/src/scout_base_node.cpp
index 2308887..3aa1a4c 100644
--- a/scout_base/src/scout_base_node.cpp
+++ b/scout_base/src/scout_base_node.cpp
@@ -1,5 +1,7 @@
-#include <nav_msgs/Odometry.h>
+#include <memory>
+
 #include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
 #include <sensor_msgs/JointState.h>
 #include <tf/transform_broadcaster.h>
 
@@ -8,11 +10,11 @@
 
 using namespace westonrobot;
 
-ScoutBase robot;
+std::shared_ptr<ScoutBase> robot;
 
 void DetachRobot(int signal) {
-  robot.Disconnect();
-  robot.Terminate();
+  robot->Disconnect();
+  robot->Terminate();
 }
 
 int main(int argc, char **argv) {
@@ -22,8 +24,14 @@ int main(int argc, char **argv) {
 
   std::signal(SIGINT, DetachRobot);
 
+  // check wether controlling scout mini
+  bool is_scout_mini = false;
+  private_node.param<bool>("is_scout_mini", is_scout_mini, false);
+  std::cout << "Working as scout mini: " << is_scout_mini << std::endl;
+
   // instantiate a robot object
-  ScoutROSMessenger messenger(&robot, &node);
+  robot = std::make_shared<ScoutBase>(is_scout_mini);
+  ScoutROSMessenger messenger(robot.get(), &node);
 
   // fetch parameters before connecting to robot
   std::string port_name;
@@ -41,10 +49,10 @@ int main(int argc, char **argv) {
   if (!messenger.simulated_robot_) {
     // connect to robot and setup ROS subscription
     if (port_name.find("can") != std::string::npos) {
-      robot.Connect(port_name);
+      robot->Connect(port_name);
       ROS_INFO("Using CAN bus to talk with the robot");
     } else {
-      robot.Connect(port_name, 115200);
+      robot->Connect(port_name, 115200);
       ROS_INFO("Using UART to talk with the robot");
     }
   }