solved merge conflicts

scout_base/src/scout_base_sim_node.cpp

@@ -1,47 +0,0 @@
-#include <string>
-
-#include <ros/ros.h>
-#include <sensor_msgs/JointState.h>
-#include <tf/transform_broadcaster.h>
-#include <nav_msgs/Odometry.h>
-
-#include "wrp_sdk/platforms/scout/scout_base.hpp"
-#include "scout_base/scout_messenger.hpp"
-
-using namespace westonrobot;
-
-int main(int argc, char **argv)
-{
-    // setup ROS node
-    ros::init(argc, argv, "scout_odom");
-    ros::NodeHandle node(""), private_node("~");
-
-    // instantiate a robot object
-    ScoutBase robot;
-    ScoutROSMessenger messenger(&robot, &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_, true);
-    private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);
-
-    // no connection for simulated robot
-    // setup ROS subscription
-    messenger.SetupSubscription();
-
-    // publish robot state at 50Hz while listening to twist commands
-    double linear, angular;
-    ros::Rate rate_50hz(50); // 50Hz
-    while (true)
-    {
-        messenger.GetCurrentMotionCmdForSim(linear, angular);
-        messenger.PublishSimStateToROS(linear, angular);
-        ros::spinOnce();
-        rate_50hz.sleep();
-    }
-
-    return 0;
-}

scout_base/src/scout_messenger.cpp

@@ -13,102 +13,86 @@
 
 #include "scout_msgs/ScoutStatus.h"
 
-#include <ros/console.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()
-{
+ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh)
+    : scout_(scout), nh_(nh) {}
+
+void ScoutROSMessenger::SetupSubscription() {
   // odometry publisher
   odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_frame_, 50);
-    status_publisher_ = nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
+  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); //不启用平滑包则订阅“cmd_vel”
-    light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
+  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_)
-    {
+void ScoutROSMessenger::TwistCmdCallback(
+    const geometry_msgs::Twist::ConstPtr &msg) {
+  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();
   }
   // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
 }
 
-void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, double &angular)
-{
+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::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg)
-{
-    if (!simulated_robot_)
-    {
-        if (msg->enable_cmd_light_control)
-        {
+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:
-            {
+      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;
@@ -116,27 +100,21 @@ void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstP
       }
 
       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;
@@ -157,8 +135,7 @@ 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;
@@ -166,39 +143,32 @@ void ScoutROSMessenger::PublishStateToROS()
 
   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.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_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);
-    // ROS_DEBUG("dt is %lg",dt);
-    std::cout<<"dt is"<<dt<< std::endl;
-    ROS_INFO("dt is %lg",dt);
 
   // record time for next integration
   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;
   }
-    double dt = (current_time_ - last_time_).toSec();
-    
-    // double dt = 1.0 / sim_control_rate_;
   ROS_INFO("dt is %lg",dt);
-    // double test_time = current_time_.toSec();
-    // ROS_INFO("current time is %lg",test_time);
 
   // publish scout state message
   scout_msgs::ScoutStatus status_msg;
@@ -217,24 +187,28 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
   // {
   //     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.motor_states[i].temperature =
+  //     state.motor_states[i].temperature;
   // }
 
   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;
+  // 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(linear, angular, dt);
+
+  // record time for next integration
   last_time_ = current_time_;
 }
 
-void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt)
-{
+void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
+                                             double dt) {
   // perform numerical integration to get an estimation of pose
   linear_speed_ = linear;
   angular_speed_ = angular;

scout_base/urdf/agilex_scout.urdf

@@ -1,72 +0,0 @@
-<!--
-  Copyright 2016 The Cartographer Authors
-
-  Licensed under the Apache License, Version 2.0 (the "License");
-  you may not use this file except in compliance with the License.
-  You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-  Unless required by applicable law or agreed to in writing, software
-  distributed under the License is distributed on an "AS IS" BASIS,
-  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-  See the License for the specific language governing permissions and
-  limitations under the License.
-
-  Reference:
-   [1] https://github.com/IntelRealSense/librealsense/blob/master/doc/d435i.md
-
--->
-
-<robot name="agilex_scout_rs16">
-  <material name="orange">
-    <color rgba="1.0 0.5 0.2 1" />
-  </material>
-  <material name="gray">
-    <color rgba="0.2 0.2 0.2 1" />
-  </material>
-
-  <!-- Robosense16 -->
-  <link name="rslidar">
-    <visual>
-      <origin xyz="0.0 0.0 0.0" />
-      <geometry>
-        <cylinder length="0.07" radius="0.05" />
-      </geometry>
-      <material name="orange" />
-    </visual>
-  </link>
-
-  <!-- Realsense D435i -->
-  <link name="imu_link">
-    <visual>
-      <origin xyz="0.0 0.0 0.0" />
-      <geometry>
-        <box size="0.06 0.04 0.02" />
-      </geometry>
-      <material name="orange" />
-    </visual>
-  </link>
-
-  <link name="base_link" />
-
-  <joint name="rslidar_joint" type="fixed">
-    <parent link="base_link" />
-    <child link="rslidar" />
-    <origin xyz="0.25 0.0 0.29" rpy="0 0 0" />
-  </joint>
-
-  <joint name="imu_joint" type="fixed">
-    <parent link="base_link" />
-    <child link="imu_link" />
-    <origin xyz="0 0 0" rpy="0 0 0" />
-  </joint>
-
-  <!-- 
-  <joint name="imu_joint" type="fixed">
-    <parent link="base_link" />
-    <child link="camera_imu_optical_frame" />
-    <origin xyz="0 0 0.0" rpy="-1.57 0 -1.57" />
-  </joint>
-  -->
-</robot>