From 601ba3e0ca076a7a964a3f1755dfdfe8f544d28d Mon Sep 17 00:00:00 2001
From: Ruixiang Du <ruixiang.du@westonrobot.com>
Date: Fri, 22 Oct 2021 13:45:38 +0800
Subject: [PATCH] updated pkg for sim support

---
 scout_base/CMakeLists.txt                     | 10 ++++-
 .../include/scout_base/scout_messenger.hpp    | 41 +++++++++----------
 scout_base/package.xml                        |  8 +++-
 scout_base/src/scout_base_node.cpp            |  4 +-
 4 files changed, 36 insertions(+), 27 deletions(-)

diff --git a/scout_base/CMakeLists.txt b/scout_base/CMakeLists.txt
index 7693fb8..e200133 100644
--- a/scout_base/CMakeLists.txt
+++ b/scout_base/CMakeLists.txt
@@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
     sensor_msgs
     std_msgs
     geometry_msgs
+    tf
     tf2 
     tf2_ros
     ugv_sdk
@@ -25,7 +26,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES scout_messenger
-  CATKIN_DEPENDS ugv_sdk scout_msgs
+  CATKIN_DEPENDS ugv_sdk scout_msgs tf tf2 tf2_ros
 #   DEPENDS Boost
 )
 
@@ -39,8 +40,13 @@ include_directories(
     include
     ${catkin_INCLUDE_DIRS})
 
+add_library(scout_messenger INTERFACE)
+target_include_directories(scout_messenger INTERFACE
+	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+
 add_executable(scout_base_node src/scout_base_node.cpp)
-target_link_libraries(scout_base_node ${catkin_LIBRARIES})
+target_link_libraries(scout_base_node scout_messenger ${catkin_LIBRARIES})
 
 #############
 ## Install ##
diff --git a/scout_base/include/scout_base/scout_messenger.hpp b/scout_base/include/scout_base/scout_messenger.hpp
index f168354..327cc03 100644
--- a/scout_base/include/scout_base/scout_messenger.hpp
+++ b/scout_base/include/scout_base/scout_messenger.hpp
@@ -15,6 +15,7 @@
 
 #include <ros/ros.h>
 #include <nav_msgs/Odometry.h>
+#include <tf/transform_datatypes.h>
 #include <tf2_ros/transform_broadcaster.h>
 
 #include "scout_msgs/ScoutStatus.h"
@@ -33,9 +34,18 @@ class ScoutMessenger {
   void SetBaseFrame(std::string frame) { base_frame_ = frame; }
   void SetOdometryTopicName(std::string name) { odom_topic_name_ = name; }
 
-  void SetSimulationMode(int loop_rate) {
-    simulated_robot_ = true;
-    sim_control_rate_ = loop_rate;
+  void SetSimulationMode() { simulated_robot_ = true; }
+
+  void 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 Run() {
@@ -50,6 +60,11 @@ class ScoutMessenger {
     }
   }
 
+  void Update() {
+    PublishStateToROS();
+    ros::spinOnce();
+  }
+
  private:
   std::shared_ptr<ScoutType> scout_;
   ros::NodeHandle *nh_;
@@ -59,7 +74,6 @@ class ScoutMessenger {
   std::string odom_topic_name_;
 
   bool simulated_robot_ = false;
-  int sim_control_rate_ = 50;
 
   std::mutex twist_mutex_;
   geometry_msgs::Twist current_twist_;
@@ -78,25 +92,8 @@ class ScoutMessenger {
   ros::Time last_time_;
   ros::Time current_time_;
 
-  void 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 TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) {
-    if (!simulated_robot_) {
-      SetScoutMotionCommand(scout_, msg);
-    } else {
-      std::lock_guard<std::mutex> guard(twist_mutex_);
-      current_twist_ = *msg.get();
-    }
+    SetScoutMotionCommand(scout_, msg);
     ROS_INFO("Cmd received:%f, %f", msg->linear.x, msg->angular.z);
   }
 
diff --git a/scout_base/package.xml b/scout_base/package.xml
index dab095a..eeef7a2 100644
--- a/scout_base/package.xml
+++ b/scout_base/package.xml
@@ -2,7 +2,7 @@
 <package>
   <name>scout_base</name>
   <version>0.3.3</version>
-  <description>AgileX Scout robot driver</description>
+  <description>Scout robot ROS interface</description>
 
   <author email="ruixiang.du@westonrobot.com">Ruixiang Du</author>
   <maintainer email="info@westonrobot.com">WestonRobot</maintainer>
@@ -19,6 +19,9 @@
   <build_depend>roslaunch</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>ugv_sdk</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
   <run_depend>controller_manager</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>scout_msgs</run_depend>
@@ -26,5 +29,8 @@
   <run_depend>sensor_msgs</run_depend>
   <run_depend>topic_tools</run_depend>
   <run_depend>ugv_sdk</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_ros</run_depend>
   <export></export>
 </package>
diff --git a/scout_base/src/scout_base_node.cpp b/scout_base/src/scout_base_node.cpp
index e89d1d8..10774c4 100644
--- a/scout_base/src/scout_base_node.cpp
+++ b/scout_base/src/scout_base_node.cpp
@@ -77,7 +77,7 @@ int main(int argc, char **argv) {
     messenger->SetOdometryFrame(odom_frame);
     messenger->SetBaseFrame(base_frame);
     messenger->SetOdometryTopicName(odom_topic_name);
-    if (is_simulated) messenger->SetSimulationMode(sim_rate);
+    if (is_simulated) messenger->SetSimulationMode();
 
     // connect to robot and setup ROS subscription
     if (port_name.find("can") != std::string::npos) {
@@ -120,7 +120,7 @@ int main(int argc, char **argv) {
     messenger->SetOdometryFrame(odom_frame);
     messenger->SetBaseFrame(base_frame);
     messenger->SetOdometryTopicName(odom_topic_name);
-    if (is_simulated) messenger->SetSimulationMode(sim_rate);
+    if (is_simulated) messenger->SetSimulationMode();
 
     // connect to robot and setup ROS subscription
     if (port_name.find("can") != std::string::npos) {