Pd dev

scout_base/include/scout_base/scout_params.hpp

@@ -5,31 +5,29 @@
  * Description: 
  * 
  * Copyright (c) 2020 Ruixiang Du (rdu)
- */ 
+ */
 
 #ifndef SCOUT_PARAMS_HPP
 #define SCOUT_PARAMS_HPP
 
-#include <cstdint>
-
 namespace westonrobot
 {
-struct ScoutParams
-{
-    /* Scout Parameters */
-    static constexpr double max_steer_angle = 30.0; // in degree
+    struct ScoutParams
+    {
+        /* Scout Parameters */
+        static constexpr double max_steer_angle = 30.0; // in degree
 
-    static constexpr double track = 0.58306;        // in meter (left & right wheel distance)
-    static constexpr double wheelbase = 0.498;    // in meter (front & rear wheel distance)
-    static constexpr double wheel_radius = 0.165; // in meter
+        static constexpr double track = 0.58306;      // in meter (left & right wheel distance)
+        static constexpr double wheelbase = 0.498;    // in meter (front & rear wheel distance)
+        static constexpr double wheel_radius = 0.165; // in meter
 
-    // from user manual v1.2.8 P18
-    // max linear velocity: 1.5 m/s
-    // max angular velocity: 0.7853 rad/s
-    static constexpr double max_linear_speed = 1.5;  // in m/s
-    static constexpr double max_angular_speed = 0.7853; // in rad/s
-    static constexpr double max_speed_cmd = 10.0;       // in rad/s
-};
+        // from user manual v1.2.8 P18
+        // max linear velocity: 1.5 m/s
+        // max angular velocity: 0.7853 rad/s
+        static constexpr double max_linear_speed = 1.5;     // in m/s
+        static constexpr double max_angular_speed = 0.7853; // in rad/s
+        static constexpr double max_speed_cmd = 10.0;       // in rad/s
+    };
 } // namespace westonrobot
 
 #endif /* SCOUT_PARAMS_HPP */

scout_base/launch/scout_base.launch

@@ -11,16 +11,14 @@
     -->
     <arg name="port_name" default="can0" />
     <arg name="simulated_robot" default="false" />
-
-    <arg name ="odom_topic_name" default = "odom"/>
+    <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)" />
+        <param name="odom_topic_name" type="string" value="$(arg odom_topic_name)" />
     </node>
 
 </launch>

scout_base/src/scout_messenger.cpp

@@ -13,244 +13,270 @@
 
 #include "scout_msgs/ScoutStatus.h"
 
+namespace westonrobot
+{
+  ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh)
+      : scout_(nullptr), nh_(nh) {}
 
-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()
+  {
+    // odometry publisher
+    odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
+    status_publisher_ =
+        nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
 
-ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh)
-    : scout_(scout), nh_(nh) {}
-
-void ScoutROSMessenger::SetupSubscription() {
-  // odometry publisher
-  odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
-  status_publisher_ =
-      nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
-
-  // cmd subscriber
-  motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>(
-      "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this);
-  light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
-      "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
-}
-
-void ScoutROSMessenger::TwistCmdCallback(
-    const geometry_msgs::Twist::ConstPtr &msg) {
-  if (!simulated_robot_) {
-    scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
-  } else {
-    std::lock_guard<std::mutex> guard(twist_mutex_);
-    current_twist_ = *msg.get();
+    // cmd subscriber
+    motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>(
+        "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this);
+    light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
+        "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
   }
-  // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
-}
 
-void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear,
-                                                  double &angular) {
-  std::lock_guard<std::mutex> guard(twist_mutex_);
-  linear = current_twist_.linear.x;
-  angular = current_twist_.angular.z;
-}
+  void ScoutROSMessenger::TwistCmdCallback(
+      const geometry_msgs::Twist::ConstPtr &msg)
+  {
+    if (!simulated_robot_)
+    {
+      scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
+    }
+    else
+    {
+      std::lock_guard<std::mutex> guard(twist_mutex_);
+      current_twist_ = *msg.get();
+    }
+    // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
+  }
 
-void ScoutROSMessenger::LightCmdCallback(
-    const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
-  if (!simulated_robot_) {
-    if (msg->enable_cmd_light_control) {
-      ScoutLightCmd cmd;
+  void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear,
+                                                    double &angular)
+  {
+    std::lock_guard<std::mutex> guard(twist_mutex_);
+    linear = current_twist_.linear.x;
+    angular = current_twist_.angular.z;
+  }
 
-      switch (msg->front_mode) {
-        case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
+  void ScoutROSMessenger::LightCmdCallback(
+      const scout_msgs::ScoutLightCmd::ConstPtr &msg)
+  {
+    if (!simulated_robot_)
+    {
+      if (msg->enable_cmd_light_control)
+      {
+        ScoutLightCmd cmd;
+
+        switch (msg->front_mode)
+        {
+        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) {
-        case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
+        switch (msg->rear_mode)
+        {
+        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;
         }
+        }
+
+        scout_->SetLightCommand(cmd);
+      }
+      else
+      {
+        scout_->DisableLightCmdControl();
       }
-
-      scout_->SetLightCommand(cmd);
-    } else {
-      scout_->DisableLightCmdControl();
     }
-  } else {
-    std::cout << "simulated robot received light control cmd" << std::endl;
+    else
+    {
+      std::cout << "simulated robot received light control cmd" << std::endl;
+    }
   }
-}
 
+  void ScoutROSMessenger::PublishStateToROS()
+  {
+    current_time_ = ros::Time::now();
+    double dt = (current_time_ - last_time_).toSec();
 
-void ScoutROSMessenger::PublishStateToROS() {
-  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;
+    }
 
-  static bool init_run = true;
-  if (init_run) {
+    auto state = scout_->GetScoutState();
+
+    // publish scout state message
+    scout_msgs::ScoutStatus status_msg;
+
+    status_msg.header.stamp = current_time_;
+
+    status_msg.linear_velocity = state.linear_velocity;
+    status_msg.angular_velocity = state.angular_velocity;
+
+    status_msg.base_state = state.base_state;
+    status_msg.control_mode = state.control_mode;
+    status_msg.fault_code = state.fault_code;
+    status_msg.battery_voltage = state.battery_voltage;
+
+    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 = state.light_control_enabled;
+    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_publisher_.publish(status_msg);
+
+    // publish odometry and tf
+    PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt);
+
+    // record time for next integration
     last_time_ = current_time_;
-    init_run = false;
-    return;
   }
 
-  auto state = scout_->GetScoutState();
+  void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
+  {
+    current_time_ = ros::Time::now();
 
-  // publish scout state message
-  scout_msgs::ScoutStatus status_msg;
+    double dt = (current_time_ - last_time_).toSec();
 
-  status_msg.header.stamp = current_time_;
+    static bool init_run = true;
+    if (init_run)
+    {
+      last_time_ = current_time_;
+      init_run = false;
+      return;
+    }
 
-  status_msg.linear_velocity = state.linear_velocity;
-  status_msg.angular_velocity = state.angular_velocity;
+    // publish scout state message
+    scout_msgs::ScoutStatus status_msg;
 
-  status_msg.base_state = state.base_state;
-  status_msg.control_mode = state.control_mode;
-  status_msg.fault_code = state.fault_code;
-  status_msg.battery_voltage = state.battery_voltage;
+    status_msg.header.stamp = current_time_;
 
-  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.linear_velocity = linear;
+    status_msg.angular_velocity = angular;
 
-  status_msg.light_control_enabled = state.light_control_enabled;
-  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_publisher_.publish(status_msg);
+    status_msg.base_state = 0x00;
+    status_msg.control_mode = 0x01;
+    status_msg.fault_code = 0x00;
+    status_msg.battery_voltage = 29.5;
 
-  // publish odometry and tf
-  PublishOdometryToROS(state.linear_velocity, state.angular_velocity, dt);
+    // 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;
+    // }
 
-  // record time for next integration
-  last_time_ = current_time_;
-}
+    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;
 
-void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
-  current_time_ = ros::Time::now();
+    status_publisher_.publish(status_msg);
 
-  double dt = (current_time_ - last_time_).toSec();
+    // publish odometry and tf
+    PublishOdometryToROS(linear, angular, dt);
 
-  static bool init_run = true;
-  if (init_run) {
+    // record time for next integration
     last_time_ = current_time_;
-    init_run = false;
-    return;
   }
-  
 
-  // publish scout state message
-  scout_msgs::ScoutStatus status_msg;
+  void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
+                                               double dt)
+  {
+    // perform numerical integration to get an estimation of pose
+    linear_speed_ = linear;
+    angular_speed_ = angular;
 
-  status_msg.header.stamp = current_time_;
+    double d_x = linear_speed_ * std::cos(theta_) * dt;
+    double d_y = linear_speed_ * std::sin(theta_) * dt;
+    double d_theta = angular_speed_ * dt;
 
-  status_msg.linear_velocity = linear;
-  status_msg.angular_velocity = angular;
+    position_x_ += d_x;
+    position_y_ += d_y;
+    theta_ += d_theta;
 
-  status_msg.base_state = 0x00;
-  status_msg.control_mode = 0x01;
-  status_msg.fault_code = 0x00;
-  status_msg.battery_voltage = 29.5;
+    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_);
 
-  // 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;
-  // }
+    // publish tf transformation
+    geometry_msgs::TransformStamped tf_msg;
+    tf_msg.header.stamp = current_time_;
+    tf_msg.header.frame_id = odom_frame_;
+    tf_msg.child_frame_id = base_frame_;
 
-  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;
+    tf_msg.transform.translation.x = position_x_;
+    tf_msg.transform.translation.y = position_y_;
+    tf_msg.transform.translation.z = 0.0;
+    tf_msg.transform.rotation = odom_quat;
 
-  status_publisher_.publish(status_msg);
+    tf_broadcaster_.sendTransform(tf_msg);
 
-  // publish odometry and tf
-  PublishOdometryToROS(linear, angular, dt);
+    // publish odometry and tf messages
+    nav_msgs::Odometry odom_msg;
+    odom_msg.header.stamp = current_time_;
+    odom_msg.header.frame_id = "testes";
+    odom_msg.child_frame_id = base_frame_;
 
-  // record time for next integration
-  last_time_ = current_time_;
-}
+    odom_msg.pose.pose.position.x = position_x_;
+    odom_msg.pose.pose.position.y = position_y_;
+    odom_msg.pose.pose.position.z = 0.0;
+    odom_msg.pose.pose.orientation = odom_quat;
 
-void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
-                                             double dt) {
-  // perform numerical integration to get an estimation of pose
-  linear_speed_ = linear;
-  angular_speed_ = angular;
+    odom_msg.twist.twist.linear.x = linear_speed_;
+    odom_msg.twist.twist.linear.y = 0.0;
+    odom_msg.twist.twist.angular.z = angular_speed_;
 
-  double d_x = linear_speed_ * std::cos(theta_) * dt;
-  double d_y = linear_speed_ * std::sin(theta_) * dt;
-  double d_theta = angular_speed_ * dt;
-
-  position_x_ += d_x;
-  position_y_ += d_y;
-  theta_ += d_theta;
-
-  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_);
-
-  // publish tf transformation
-  geometry_msgs::TransformStamped tf_msg;
-  tf_msg.header.stamp = current_time_;
-  tf_msg.header.frame_id = odom_frame_;
-  tf_msg.child_frame_id = base_frame_;
-
-  tf_msg.transform.translation.x = position_x_;
-  tf_msg.transform.translation.y = position_y_;
-  tf_msg.transform.translation.z = 0.0;
-  tf_msg.transform.rotation = odom_quat;
-
-  tf_broadcaster_.sendTransform(tf_msg);
-
-  // publish odometry and tf messages
-  nav_msgs::Odometry odom_msg;
-  odom_msg.header.stamp = current_time_;
-  odom_msg.header.frame_id = "testes";
-  odom_msg.child_frame_id = base_frame_;
-
-  odom_msg.pose.pose.position.x = position_x_;
-  odom_msg.pose.pose.position.y = position_y_;
-  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.y = 0.0;
-  odom_msg.twist.twist.angular.z = angular_speed_;
-
-  odom_publisher_.publish(odom_msg);
-}
-}  // namespace westonrobot
+    odom_publisher_.publish(odom_msg);
+  }
+} // namespace westonrobot

scout_bringup/launch/scout_minimal.launch

@@ -2,17 +2,17 @@
 
     <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="model_xacro" default="$(find scout_description)/urdf/scout_v2.xacro" />
+    <arg name="odom_topic_name" default="odom" />
 
-    <include file="$(find scout_base)/launch/scout_base.launch" >
+    <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 file="$(find scout_description)/launch/description.launch">
+        <arg name="model_xacro" default="$(arg model_xacro)" />
     </include>
+
 </launch>

scout_bringup/launch/scout_minimal_uart.launch

@@ -2,17 +2,18 @@
 
     <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="model_xacro" default="$(find scout_description)/urdf/scout_v2.xacro" />
+    <arg name="odom_topic_name" default="odom" />
 
-    <include file="$(find scout_base)/launch/scout_base.launch" >
+    <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 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>