tested work with both scout_base_node and scout_messenger

scout_base/CMakeLists.txt

@@ -24,7 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 catkin_package(
   INCLUDE_DIRS include
-  # LIBRARIES scout_messenger
+  LIBRARIES scout_messenger
   CATKIN_DEPENDS ugv_sdk scout_msgs
 #   DEPENDS Boost
 )
@@ -40,13 +40,12 @@ include_directories(
     ${catkin_INCLUDE_DIRS}
 )
 
-# add_library(scout_messenger STATIC src/scout_messenger.cpp)
+add_library(scout_messenger STATIC src/scout_messenger.cpp)
-# target_link_libraries(scout_messenger ${catkin_LIBRARIES})
+target_link_libraries(scout_messenger ${catkin_LIBRARIES})
-# add_dependencies(scout_messenger ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(scout_messenger ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 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})
-add_dependencies(scout_base_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 #############
 ## Install ##

scout_base/include/scout_base/scout_messenger.hpp

@@ -18,7 +18,7 @@
 #include <tf2_ros/transform_broadcaster.h>
 
 #include "scout_msgs/ScoutLightCmd.h"
-#include "ugv_sdk/scout/scout_base.hpp"
+#include "ugv_sdk/scout_base.hpp"
 
 namespace westonrobot
 {

scout_base/src/scout_base_node.cpp

@@ -5,175 +5,58 @@
 #include <sensor_msgs/JointState.h>
 #include <tf/transform_broadcaster.h>
 
-#include "scout_msgs/ScoutMotionState.h"
-#include "scout_msgs/ScoutLightCmd.h"
-#include "scout_msgs/ScoutCheckActuatorState.h"
-#include "scout_msgs/ScoutCheckLightState.h"
-#include "scout_msgs/ScoutCheckRCState.h"
-#include "scout_msgs/ScoutCheckSystemState.h"
-
 #include "ugv_sdk/scout_base.hpp"
+#include "scout_base/scout_messenger.hpp"
 
 using namespace westonrobot;
 
-typedef enum {
-  SYSTEM_MSG = 0,
-  MOTION_MSG,
-  LIGHT_MSG,
-  RC_MSG,
-  ACTUATOR_MSG
-} MsgsType;
-
 std::shared_ptr<ScoutBase> robot;
 
-void ControlCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) {
-  robot->SetMotionCommand(msg->linear.x, msg->angular.z);
-  ROS_INFO("received motion control cmd");
-}
-
-void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
-  ScoutLightCmd cmd;
-  cmd.cmd_ctrl_allowed = msg->cmd_ctrl_allowed;
-  cmd.front_mode = (LightMode)msg->front_mode;
-  cmd.front_custom_value = msg->front_custom_value;
-  cmd.rear_mode = (LightMode)msg->rear_mode;
-  cmd.rear_custom_value = msg->rear_custom_value;
-  robot->SetLightCommand(cmd);
-  ROS_INFO("received light control cmd");
-}
-
-void PublishMsgTOROS(const ros::Publisher *pub, MsgsType msg_type) {
-  ScoutState scout_state = robot->GetScoutState();
-
-  switch (msg_type) {
-    case SYSTEM_MSG: {
-      break;
-    }
-    case MOTION_MSG: {
-      scout_msgs::ScoutMotionState msg;
-      msg.header.stamp = ros::Time::now();
-      msg.linear_velocity = scout_state.motion_state.linear_velocity;
-      msg.angular_velocity = scout_state.motion_state.angular_velocity;
-      pub->publish(msg);
-      break;
-    }
-    case LIGHT_MSG: {
-      break;
-    }
-    case RC_MSG: {
-      break;
-    }
-    case ACTUATOR_MSG: {
-      break;
-    }
-    default:
-      break;
-  }
-}
-
-bool CheckActuatorState(scout_msgs::ScoutCheckActuatorState::Request &req,
-                        scout_msgs::ScoutCheckActuatorState::Response &res) {
-  ScoutState scout_state = robot->GetScoutState();
-
-  uint8_t motor_id = req.motor_id;
-  res.rpm = scout_state.actuator_hs_state[motor_id].rpm;
-  res.current = scout_state.actuator_hs_state[motor_id].current;
-  res.pulse_count = scout_state.actuator_hs_state[motor_id].pulse_count;
-
-  res.driver_voltage = scout_state.actuator_ls_state[motor_id].driver_voltage;
-  res.driver_temperature =
-      scout_state.actuator_ls_state[motor_id].driver_temperature;
-  res.motor_temperature =
-      scout_state.actuator_ls_state[motor_id].motor_temperature;
-  res.driver_state == scout_state.actuator_ls_state[motor_id].driver_state;
-
-  ROS_INFO("Actuator %d state check request",motor_id);
-  return true;
-}
-
-bool CheckLightState(scout_msgs::ScoutCheckLightState::Request &req,
-                     scout_msgs::ScoutCheckLightState::Response &res) {
-  ScoutState scout_state = robot->GetScoutState();
-
-  res.cmd_ctrl_allowed = scout_state.light_state.cmd_ctrl_allowed;
-  res.front_mode = scout_state.light_state.front_light.mode;
-  res.front_custom_value = scout_state.light_state.front_light.custom_value;
-  res.rear_mode = scout_state.light_state.rear_light.mode;
-  res.rear_custom_value = scout_state.light_state.rear_light.custom_value;
-
-  ROS_INFO("Light state check request");
-  return true;
-}
-
-bool CheckRCState(scout_msgs::ScoutCheckRCState::Request &req,
-                  scout_msgs::ScoutCheckRCState::Response &res) {
-  ScoutState scout_state = robot->GetScoutState();
-
-  res.swa = scout_state.rc_state.swa;
-  res.swb = scout_state.rc_state.swb;
-  res.swc = scout_state.rc_state.swc;
-  res.swd = scout_state.rc_state.swd;
-
-  res.stick_right_v = scout_state.rc_state.stick_right_v;
-  res.stick_right_h = scout_state.rc_state.stick_right_h;
-  res.stick_left_v = scout_state.rc_state.stick_left_v;
-  res.stick_left_h = scout_state.rc_state.stick_left_h;
-  res.var_a = scout_state.rc_state.var_a;
-
-  ROS_INFO("Light state check request");
-  return true;
-}
-
-bool CheckSystemState(scout_msgs::ScoutCheckSystemState::Request &req,
-                  scout_msgs::ScoutCheckSystemState::Response &res) {
-  ScoutState scout_state = robot->GetScoutState();
-
-  res.vehicle_state = scout_state.system_state.vehicle_state;
-  res.control_mode = scout_state.system_state.control_mode;
-  res.battery_voltage = scout_state.system_state.battery_voltage;
-  res.error_code = scout_state.system_state.error_code;
-
-  ROS_INFO("Light state check request");
-  return true;
-}
-
 int main(int argc, char **argv) {
   // setup ROS node
   ros::init(argc, argv, "scout_node");
-  // ros::NodeHandle node;
   ros::NodeHandle node(""), private_node("~");
-  // connect robot device
-  std::string device_name;
-  private_node.param<std::string>("port_name", device_name, std::string("can0"));
 
+  // instantiate a robot object
   robot = std::make_shared<ScoutBase>();
-  robot->Connect(device_name);
+  ScoutROSMessenger messenger(robot.get(), &node);
+
+  // fetch parameters before connecting to robot
+  std::string port_name;
+  private_node.param<std::string>("port_name", port_name, std::string("can0"));
+  private_node.param<std::string>("odom_frame", messenger.odom_frame_,
+                                  std::string("odom"));
+  private_node.param<std::string>("base_frame", messenger.base_frame_,
+                                  std::string("base_link"));
+  private_node.param<bool>("simulated_robot", messenger.simulated_robot_,
+                           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_) {
+    // connect to robot and setup ROS subscription
+    if (port_name.find("can") != std::string::npos) {
+      robot->Connect(port_name);
       robot->EnableCommandedMode();
-
+      ROS_INFO("Using CAN bus to talk with the robot");
-  // publisher
-  ros::Publisher state_pub = node.advertise<scout_msgs::ScoutMotionState>(
-      "/scout_motion_state", 10);
-
-  // subscriber
-  ros::Subscriber motion_cmd_sub =
-      node.subscribe<geometry_msgs::Twist>("/cmd_vel", 5, ControlCmdCallback);
-  ros::Subscriber light_cmd_sub = node.subscribe<scout_msgs::ScoutLightCmd>(
-      "/scout_light_control", 5, LightCmdCallback);
-
-  // servicer
-  ros::ServiceServer service1 =
-      node.advertiseService("check_scout_actuator", CheckActuatorState);
-  ros::ServiceServer service2 =
-      node.advertiseService("check_scout_light", CheckLightState);
-  ros::ServiceServer service3 =
-      node.advertiseService("check_scout_RC", CheckRCState);
-  ros::ServiceServer service4 =
-      node.advertiseService("check_scout_system", CheckSystemState);
-
-  ros::Rate loop_rate(50);
-  while (ros::ok()) {
-    PublishMsgTOROS(&state_pub, MOTION_MSG);
-    ros::spinOnce();
-    loop_rate.sleep();
     } 
+  }
+  messenger.SetupSubscription();
+
+  // publish robot state at 50Hz while listening to twist commands
+  ros::Rate rate(50);
+  while (true) {
+    if (!messenger.simulated_robot_) {
+      messenger.PublishStateToROS();
+    } else {
+      double linear, angular;
+      messenger.GetCurrentMotionCmdForSim(linear, angular);
+      messenger.PublishSimStateToROS(linear, angular);
+    }
+    ros::spinOnce();
+    rate.sleep();
+  }
+
+  return 0;
 }

scout_base/src/scout_messenger.cpp

@@ -63,7 +63,7 @@ namespace westonrobot
   {
     if (!simulated_robot_)
     {
-      if (msg->enable_cmd_light_control)
+      if (msg->cmd_ctrl_allowed)
       {
         ScoutLightCmd cmd;
 
@@ -71,57 +71,57 @@ namespace westonrobot
         {
         case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
         {
-          cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
+          cmd.front_mode = CONST_OFF;
           break;
         }
         case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
         {
-          cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
+          cmd.front_mode = CONST_ON;
           break;
         }
         case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
         {
-          cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
+          cmd.front_mode = BREATH;
           break;
         }
         case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
         {
-          cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
+          cmd.front_mode = CUSTOM;
           cmd.front_custom_value = msg->front_custom_value;
           break;
         }
         }
-
+        // not meant to be controlled by user for now
-        switch (msg->rear_mode)
+        // switch (msg->rear_mode)
-        {
+        // {
-        case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
+        // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
-        {
+        // {
-          cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
+        //   cmd.rear_mode = CONST_OFF;
-          break;
+        //   break;
-        }
+        // }
-        case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
+        // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
-        {
+        // {
-          cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
+        //   cmd.rear_mode = CONST_ON;
-          break;
+        //   break;
-        }
+        // }
-        case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
+        // case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
-        {
+        // {
-          cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
+        //   cmd.rear_mode = BREATH;
-          break;
+        //   break;
-        }
+        // }
-        case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
+        // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
-        {
+        // {
-          cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
+        //   cmd.rear_mode = CUSTOM;
-          cmd.rear_custom_value = msg->rear_custom_value;
+        //   cmd.rear_custom_value = msg->rear_custom_value;
-          break;
+        //   break;
-        }
+        // }
-        }
+        // }
-
+        cmd.cmd_ctrl_allowed = true;
         scout_->SetLightCommand(cmd);
       }
       else
       {
-        scout_->DisableLightCmdControl();
+        scout_->DisableLightControl();
       }
     }
     else
@@ -150,32 +150,43 @@ namespace westonrobot
 
     status_msg.header.stamp = current_time_;
 
-    status_msg.linear_velocity = state.linear_velocity;
+    status_msg.linear_velocity = state.motion_state.linear_velocity;
-    status_msg.angular_velocity = state.angular_velocity;
+    status_msg.angular_velocity = state.motion_state.angular_velocity;
 
-    status_msg.base_state = state.base_state;
+    status_msg.vehicle_state = state.system_state.vehicle_state;
-    status_msg.control_mode = state.control_mode;
+    status_msg.control_mode = state.system_state.control_mode;
-    status_msg.fault_code = state.fault_code;
+    status_msg.error_code = state.system_state.error_code;
-    status_msg.battery_voltage = state.battery_voltage;
+    status_msg.battery_voltage = state.system_state.battery_voltage;
 
     for (int i = 0; i < 4; ++i)
     {
-      status_msg.motor_states[i].current = state.motor_states[i].current;
+      // actuator_hs_state
-      status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
+      uint8_t motor_id = state.actuator_hs_state[i].motor_id;
-      status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
+
+      status_msg.actuator_states[motor_id].rpm = state.actuator_hs_state[i].rpm;
+      status_msg.actuator_states[motor_id].current = state.actuator_hs_state[i].current;
+      status_msg.actuator_states[motor_id].pulse_count = state.actuator_hs_state[i].pulse_count;
+      
+      // actuator_ls_state
+      motor_id = state.actuator_ls_state[i].motor_id;
+
+      status_msg.actuator_states[motor_id].driver_voltage = state.actuator_ls_state[i].driver_voltage;
+      status_msg.actuator_states[motor_id].driver_temperature = state.actuator_ls_state[i].driver_temperature;
+      status_msg.actuator_states[motor_id].motor_temperature = state.actuator_ls_state[i].motor_temperature;
+      status_msg.actuator_states[motor_id].driver_state = state.actuator_ls_state[i].driver_state;
     }
 
-    status_msg.light_control_enabled = state.light_control_enabled;
+    status_msg.light_control_enabled = state.light_state.cmd_ctrl_allowed;
-    status_msg.front_light_state.mode = state.front_light_state.mode;
+    status_msg.front_light_state.mode = state.light_state.front_light.mode;
     status_msg.front_light_state.custom_value =
-        state.front_light_state.custom_value;
+        state.light_state.front_light.custom_value;
-    status_msg.rear_light_state.mode = state.rear_light_state.mode;
+    status_msg.rear_light_state.mode = state.light_state.rear_light.mode;
     status_msg.rear_light_state.custom_value =
-        state.front_light_state.custom_value;
+        state.light_state.rear_light.custom_value;
     status_publisher_.publish(status_msg);
 
     // publish odometry and tf
-    PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt);
+    PublishOdometryToROS(state.motion_state.linear_velocity, state.motion_state.angular_velocity, dt);
 
     // record time for next integration
     last_time_ = current_time_;
@@ -203,9 +214,9 @@ namespace westonrobot
     status_msg.linear_velocity = linear;
     status_msg.angular_velocity = angular;
 
-    status_msg.base_state = 0x00;
+    status_msg.vehicle_state = 0x00;
     status_msg.control_mode = 0x01;
-    status_msg.fault_code = 0x00;
+    status_msg.error_code = 0x00;
     status_msg.battery_voltage = 29.5;
 
     // for (int i = 0; i < 4; ++i)