resovled conflicts

scout_base/src/imu_broadcast_node.cpp

@@ -1,72 +0,0 @@
-/* 
- * imu_broadcast_node.cpp
- * 
- * Created on: Oct 02, 2019 19:09
- * Description: 
- * 
- * Copyright (c) 2019 Ruixiang Du (rdu)
- */
-
-#include <string>
-
-#include <lcm/lcm-cpp.hpp>
-#include "lcmtypes/wescore.hpp"
-
-#include <ros/ros.h>
-#include <sensor_msgs/Imu.h>
-
-ros::Publisher imu_pub;
-
-struct MessageBroadcaster
-{
-    void IMULCMCallback(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const wescore_lcm_msgs::IMU *msg)
-    {
-        static uint64_t count = 0;
-        // std::cout << "imu msg received" << std::endl;
-        sensor_msgs::Imu imu_msg;
-        imu_msg.header.frame_id = "imu_link";
-        imu_msg.header.stamp = ros::Time::now();
-        imu_msg.header.seq = count++;
-
-        imu_msg.angular_velocity.x = msg->angular_velocity.x;
-        imu_msg.angular_velocity.y = msg->angular_velocity.y;
-        imu_msg.angular_velocity.z = msg->angular_velocity.z;
-
-        imu_msg.linear_acceleration.x = msg->linear_acceleration.x;
-        imu_msg.linear_acceleration.y = msg->linear_acceleration.y;
-        imu_msg.linear_acceleration.z = msg->linear_acceleration.z;
-
-        for (int i = 0; i < 9; ++i)
-        {
-            imu_msg.orientation_covariance[i] = msg->orientation_covariance[i];
-            imu_msg.angular_velocity_covariance[i] = msg->angular_velocity_covariance[i];
-            imu_msg.linear_acceleration_covariance[i] = msg->linear_acceleration_covariance[i];
-        }
-
-        imu_pub.publish(imu_msg);
-    }
-};
-
-int main(int argc, char **argv)
-{
-    // setup LCM
-    lcm::LCM lcm;
-
-    if (!lcm.good())
-        return 1;
-    MessageBroadcaster mb;
-    lcm.subscribe("sensor_hub_raw_imu", &MessageBroadcaster::IMULCMCallback, &mb);
-
-    // setup ROS node
-    ros::init(argc, argv, "imu_broadcast_node");
-    ros::NodeHandle nh;
-    imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 1000);
-
-    ROS_INFO("Started broadcasting");
-    while (ros::ok())
-    {
-        lcm.handleTimeout(5);
-    }
-
-    return 0;
-}

scout_base/src/scout_base_node.cpp

@@ -1,53 +1,58 @@
-#include <string>
-
+#include <nav_msgs/Odometry.h>
 #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("~");
+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);
+  // 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_, false);
+  // 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);
 
+  if (!messenger.simulated_robot_) {
     // connect to robot and setup ROS subscription
-    if (port_name.find("can") != std::string::npos)
-    {
-        robot.Connect(port_name);
-        ROS_INFO("Using CAN bus to talk with the robot");
+    if (port_name.find("can") != std::string::npos) {
+      robot.Connect(port_name);
+      ROS_INFO("Using CAN bus to talk with the robot");
+    } else {
+      robot.Connect(port_name, 115200);
+      ROS_INFO("Using UART to talk with the robot");
     }
-    else
-    {
-        robot.Connect(port_name, 115200);
-        ROS_INFO("Using UART to talk with the robot");
-    }
-    messenger.SetupSubscription();
+  }
+  messenger.SetupSubscription();
 
-    // publish robot state at 50Hz while listening to twist commands
-    ros::Rate rate_50hz(50); // 50Hz
-    while (true)
-    {
-        messenger.PublishStateToROS();
-        ros::spinOnce();
-        rate_50hz.sleep();
+  // 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;
+  return 0;
 }