Pd dev

scout_base/include/scout_base/scout_params.hpp

@@ -10,8 +10,6 @@
 #ifndef SCOUT_PARAMS_HPP
 #define SCOUT_PARAMS_HPP
 
-#include <cstdint>
-
 namespace westonrobot
 {
     struct ScoutParams

scout_base/launch/scout_base.launch

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

scout_base/src/scout_messenger.cpp

@@ -13,16 +13,16 @@
 
 #include "scout_msgs/ScoutStatus.h"
 
-
+namespace westonrobot
-namespace westonrobot {
+{
   ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh)
       : scout_(nullptr), nh_(nh) {}
 
-
   ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh)
       : scout_(scout), nh_(nh) {}
 
-void ScoutROSMessenger::SetupSubscription() {
+  void ScoutROSMessenger::SetupSubscription()
+  {
     // odometry publisher
     odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
     status_publisher_ =
@@ -36,10 +36,14 @@ void ScoutROSMessenger::SetupSubscription() {
   }
 
   void ScoutROSMessenger::TwistCmdCallback(
-    const geometry_msgs::Twist::ConstPtr &msg) {
+      const geometry_msgs::Twist::ConstPtr &msg)
-  if (!simulated_robot_) {
+  {
+    if (!simulated_robot_)
+    {
       scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
-  } else {
+    }
+    else
+    {
       std::lock_guard<std::mutex> guard(twist_mutex_);
       current_twist_ = *msg.get();
     }
@@ -47,52 +51,66 @@ void ScoutROSMessenger::TwistCmdCallback(
   }
 
   void ScoutROSMessenger::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(
-    const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
+      const scout_msgs::ScoutLightCmd::ConstPtr &msg)
-  if (!simulated_robot_) {
+  {
-    if (msg->enable_cmd_light_control) {
+    if (!simulated_robot_)
+    {
+      if (msg->enable_cmd_light_control)
+      {
         ScoutLightCmd cmd;
 
-      switch (msg->front_mode) {
+        switch (msg->front_mode)
-        case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
+        {
+        case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
+        {
           cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
           break;
         }
-        case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
+        case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
+        {
           cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
           break;
         }
-        case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
+        case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
+        {
           cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
           break;
         }
-        case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
+        case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
+        {
           cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
           cmd.front_custom_value = msg->front_custom_value;
           break;
         }
         }
 
-      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;
           break;
         }
-        case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
+        case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
+        {
           cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
           break;
         }
-        case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
+        case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
+        {
           cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
           break;
         }
-        case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
+        case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
+        {
           cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
           cmd.rear_custom_value = msg->rear_custom_value;
           break;
@@ -100,21 +118,26 @@ void ScoutROSMessenger::LightCmdCallback(
         }
 
         scout_->SetLightCommand(cmd);
-    } else {
+      }
+      else
+      {
         scout_->DisableLightCmdControl();
       }
-  } else {
+    }
+    else
+    {
       std::cout << "simulated robot received light control cmd" << std::endl;
     }
   }
 
-
+  void ScoutROSMessenger::PublishStateToROS()
-void ScoutROSMessenger::PublishStateToROS() {
+  {
     current_time_ = ros::Time::now();
     double dt = (current_time_ - last_time_).toSec();
 
     static bool init_run = true;
-  if (init_run) {
+    if (init_run)
+    {
       last_time_ = current_time_;
       init_run = false;
       return;
@@ -135,7 +158,8 @@ void ScoutROSMessenger::PublishStateToROS() {
     status_msg.fault_code = state.fault_code;
     status_msg.battery_voltage = state.battery_voltage;
 
-  for (int i = 0; i < 4; ++i) {
+    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;
@@ -157,19 +181,20 @@ void ScoutROSMessenger::PublishStateToROS() {
     last_time_ = current_time_;
   }
 
-void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
+  void ScoutROSMessenger::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) {
+    if (init_run)
+    {
       last_time_ = current_time_;
       init_run = false;
       return;
     }
 
-
     // publish scout state message
     scout_msgs::ScoutStatus status_msg;
 
@@ -208,7 +233,8 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
   }
 
   void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
-                                             double dt) {
+                                               double dt)
+  {
     // perform numerical integration to get an estimation of pose
     linear_speed_ = linear;
     angular_speed_ = angular;

scout_bringup/launch/scout_minimal.launch

@@ -3,16 +3,16 @@
     <arg name="port_name" value="can0" />
     <arg name="simulated_robot" value="false" />
     <arg name="model_xacro" default="$(find scout_description)/urdf/scout_v2.xacro" />
-    <arg name ="odom_topic_name" default = "odomsdas"/>
+    <arg name="odom_topic_name" default="odom" />
 
     <include file="$(find scout_base)/launch/scout_base.launch">
         <arg name="port_name" default="$(arg port_name)" />
         <arg name="simulated_robot" default="$(arg simulated_robot)" />
         <arg name="odom_topic_name" default="$(arg odom_topic_name)" />
-
     </include>
 
     <include file="$(find scout_description)/launch/description.launch">
         <arg name="model_xacro" default="$(arg model_xacro)" />
     </include>
+
 </launch>

scout_bringup/launch/scout_minimal_uart.launch

@@ -3,7 +3,7 @@
     <arg name="port_name" value="/dev/ttyUSB0" />
     <arg name="simulated_robot" value="false" />
     <arg name="model_xacro" default="$(find scout_description)/urdf/scout_v2.xacro" />
-    <arg name ="odom_topic_name" default = "odomsdas"/>
+    <arg name="odom_topic_name" default="odom" />
 
     <include file="$(find scout_base)/launch/scout_base.launch">
         <arg name="port_name" default="$(arg port_name)" />
@@ -15,4 +15,5 @@
     <include file="$(find scout_description)/launch/description.launch">
         <arg name="model_xacro" default="$(arg model_xacro)" />
     </include>
+
 </launch>

scout_bringup/launch/scout_teleop_keyboard.launch

@@ -1,4 +1,7 @@
 <launch>
+    <arg name="repeat_rate" value="50.0" />
+
     <node name="teleop_keybord" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen">
+        <param name="repeat_rate" value="$(arg repeat_rate)" />
     </node>
 </launch>