Merge pull request #26 from lopsided98/indigo-devel

Publish JointState messages for the wheels

Author: jacobperron

README.md

@@ -161,11 +161,13 @@ $ roslaunch ca_driver create_1.launch [publish_tf:=true]
  `dock_button` | 'dock' button is pressed ('advance' button for Create 1) | [std_msgs/Empty][empty]
  `spot_button` | 'spot' button is pressed | [std_msgs/Empty][empty]
  `ir_omni` | The IR character currently being read by the omnidirectional receiver. Value 0 means no character is being received | [std_msgs/UInt16][uint16]
+ `joint_states` | The states (position, velocity) of the drive wheel joints | [sensor_msgs/JointState][jointstate_msg]
  `mode` | The current mode of the robot (See [OI Spec][oi_spec] for details)| [ca_msgs/Mode][mode_msg]
  `odom` |  Robot odometry according to wheel encoders | [nav_msgs/Odometry][odometry]
  `wheeldrop` | At least one of the drive wheels has dropped | [std_msgs/Empty][empty]
  `/tf` | The transform from the `odom` frame to `base_footprint`. Only if the parameter `publish_tf` is `true` | [tf2_msgs/TFMessage](http://docs.ros.org/jade/api/tf2_msgs/html/msg/TFMessage.html)
 
+
 ### Subscribers
 
 Topic       | Description   | Type
@@ -226,3 +228,4 @@ Contributing to the development and maintenance of _create\_autonomy_ is encoura
 [bumper_msg]:  https://github.com/AutonomyLab/create_autonomy/blob/indigo-devel/ca_msgs/msg/Bumper.msg
 [mode_msg]:  https://github.com/AutonomyLab/create_autonomy/blob/indigo-devel/ca_msgs/msg/Mode.msg
 [chargingstate_msg]:  https://github.com/AutonomyLab/create_autonomy/blob/indigo-devel/ca_msgs/msg/ChargingState.msg
+[jointstate_msg]:  http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html

ca_driver/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   geometry_msgs
   nav_msgs
+  sensor_msgs
   tf
   ca_msgs
 )
@@ -15,13 +16,13 @@ find_package(Boost REQUIRED system thread)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES libcreate
-  CATKIN_DEPENDS roscpp std_msgs geometry_msgs nav_msgs tf ca_msgs ca_description
+  CATKIN_DEPENDS roscpp std_msgs geometry_msgs nav_msgs sensor_msgs tf ca_msgs ca_description
 )
 
 include(ExternalProject)
 ExternalProject_Add(libcreate
   GIT_REPOSITORY https://github.com/AutonomyLab/libcreate.git
-  GIT_TAG 1.2.1
+  GIT_TAG master
   PREFIX ${CATKIN_DEVEL_PREFIX}
   CONFIGURE_COMMAND cmake .
   BUILD_COMMAND make

ca_driver/include/create_driver/create_driver.h

@@ -8,6 +8,7 @@
 #include <std_msgs/UInt16.h>
 #include <std_msgs/Int16.h>
 #include <std_msgs/UInt8MultiArray.h>
+#include <sensor_msgs/JointState.h>
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <nav_msgs/Odometry.h>
@@ -42,6 +43,7 @@ class CreateDriver
   std_msgs::Float32 float32_msg_;
   std_msgs::UInt16 uint16_msg_;
   std_msgs::Int16 int16_msg_;
+  sensor_msgs::JointState joint_state_msg_;
 
   // ROS params
   double loop_hz_;
@@ -62,6 +64,7 @@ class CreateDriver
 
   bool update();
   void publishOdom();
+  void publishJointState();
   void publishBatteryInfo();
   void publishButtonPresses() const;
   void publishOmniChar();
@@ -100,6 +103,7 @@ class CreateDriver
   ros::Publisher mode_pub_;
   ros::Publisher bumper_pub_;
   ros::Publisher wheeldrop_pub_;
+  ros::Publisher wheel_joint_pub_;
 
 public:
   CreateDriver(ros::NodeHandle& nh);

ca_driver/package.xml

@@ -18,13 +18,15 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nav_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>ca_msgs</build_depend>
 
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <run_depend>nav_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>ca_msgs</run_depend>
   <run_depend>ca_description</run_depend>

ca_driver/src/create_driver.cpp

@@ -49,6 +49,12 @@ CreateDriver::CreateDriver(ros::NodeHandle& nh) : nh_(nh), priv_nh_("~")
   tf_odom_.child_frame_id = str_base_footprint;
   odom_msg_.header.frame_id = "odom";
   odom_msg_.child_frame_id = str_base_footprint;
+  joint_state_msg_.name.resize(2);
+  joint_state_msg_.position.resize(2);
+  joint_state_msg_.velocity.resize(2);
+  joint_state_msg_.effort.resize(2);
+  joint_state_msg_.name[0] = "left_wheel_joint";
+  joint_state_msg_.name[1] = "right_wheel_joint";
 
   // Populate intial covariances
   for (int i = 0; i < 36; i++)
@@ -85,6 +91,7 @@ CreateDriver::CreateDriver(ros::NodeHandle& nh) : nh_(nh), priv_nh_("~")
   mode_pub_ = nh.advertise<ca_msgs::Mode>("mode", 30);
   bumper_pub_ = nh.advertise<ca_msgs::Bumper>("bumper", 30);
   wheeldrop_pub_ = nh.advertise<std_msgs::Empty>("wheeldrop", 30);
+  wheel_joint_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
   ROS_INFO("[CREATE] Ready.");
 }
 
@@ -190,6 +197,7 @@ void CreateDriver::undockCallback(const std_msgs::EmptyConstPtr& msg)
 bool CreateDriver::update()
 {
   publishOdom();
+  publishJointState();
   publishBatteryInfo();
   publishButtonPresses();
   publishOmniChar();
@@ -255,6 +263,18 @@ void CreateDriver::publishOdom()
   odom_pub_.publish(odom_msg_);
 }
 
+void CreateDriver::publishJointState() {
+    // Publish joint states
+    static float CREATE_2_WHEEL_RADIUS = create::util::CREATE_2_WHEEL_DIAMETER / 2.0;
+
+    joint_state_msg_.header.stamp = ros::Time::now();
+    joint_state_msg_.position[0] = robot_->getLeftWheelDistance() / CREATE_2_WHEEL_RADIUS;
+    joint_state_msg_.position[1] = robot_->getRightWheelDistance() / CREATE_2_WHEEL_RADIUS;
+    joint_state_msg_.velocity[0] = robot_->getRequestedLeftWheelVel() / CREATE_2_WHEEL_RADIUS;
+    joint_state_msg_.velocity[1] = robot_->getRequestedRightWheelVel() / CREATE_2_WHEEL_RADIUS;
+    wheel_joint_pub_.publish(joint_state_msg_);
+}
+
 void CreateDriver::publishBatteryInfo()
 {
   float32_msg_.data = robot_->getVoltage();