diff --git a/README.md b/README.md index 6b840028b..72c120458 100644 --- a/README.md +++ b/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 diff --git a/ca_driver/CMakeLists.txt b/ca_driver/CMakeLists.txt index ad58cbbab..c3cce0b9d 100644 --- a/ca_driver/CMakeLists.txt +++ b/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 diff --git a/ca_driver/include/create_driver/create_driver.h b/ca_driver/include/create_driver/create_driver.h index 49e4bb86f..41cbd3a27 100644 --- a/ca_driver/include/create_driver/create_driver.h +++ b/ca_driver/include/create_driver/create_driver.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -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); diff --git a/ca_driver/package.xml b/ca_driver/package.xml index 5a179982e..c4faf4862 100644 --- a/ca_driver/package.xml +++ b/ca_driver/package.xml @@ -18,6 +18,7 @@ std_msgs geometry_msgs nav_msgs + sensor_msgs tf ca_msgs @@ -25,6 +26,7 @@ std_msgs geometry_msgs nav_msgs + sensor_msgs tf ca_msgs ca_description diff --git a/ca_driver/src/create_driver.cpp b/ca_driver/src/create_driver.cpp index e65d877ea..056d2c3b8 100644 --- a/ca_driver/src/create_driver.cpp +++ b/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("mode", 30); bumper_pub_ = nh.advertise("bumper", 30); wheeldrop_pub_ = nh.advertise("wheeldrop", 30); + wheel_joint_pub_ = nh.advertise("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();