merged with scout_sim

scout_base/CMakeLists.txt

@@ -26,7 +26,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES scout_messenger
-  CATKIN_DEPENDS scout_msgs roscpp sensor_msgs
+  CATKIN_DEPENDS scout_msgs roscpp sensor_msgs wrp_sdk
 #   DEPENDS Boost
 )
 
@@ -52,7 +52,7 @@ target_link_libraries(scout_base_node scout_messenger ${catkin_LIBRARIES})
 ## Install ##
 #############
 
-roslaunch_add_file_check(launch)
+# roslaunch_add_file_check(launch)
 
 install(TARGETS scout_messenger scout_base_node
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

scout_base/launch/scout_base_sim.launch

@@ -0,0 +1,22 @@
+<launch>
+    <!-- 
+        The robot can be controlled either through CAN bus or UART port. Make sure the hardware
+        interface is set up correctly before attempting to connect to the robot.
+        
+        You only need to specify the port name, such as "can0", "/dev/ttyUSB0". The port should 
+        operate with the following configuration:
+        
+        * CAN bus: 500k
+        * UART: 115200, Parity None, 8-bit Data, 1 Stop Bit 
+    -->
+    <arg name="simulated_robot" default="true" />
+    <arg name="control_rate" default="50" />
+
+    <node name="scout_base_node" pkg="scout_base" type="scout_base_node" output="screen">
+        <param name="simulated_robot" type="bool" value="$(arg simulated_robot)" />
+        <param name="control_rate" type="int" value="$(arg control_rate)" />
+
+        <param name="odom_frame" type="string" value="odom" />
+        <param name="base_frame" type="string" value="base_link" />
+    </node>
+</launch>

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 20Hz while listening to twist commands
-    ros::Rate rate_20hz(20); // 20Hz
-    while (true)
-    {
-        messenger.PublishStateToROS();
-        ros::spinOnce();
-        rate_20hz.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;
 }

scout_base/src/scout_base_sim_node.cpp

@@ -0,0 +1,47 @@
+#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;
+}