Skip to content

Publish JointState messages for the wheels #26

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Jun 28, 2016
Merged

Publish JointState messages for the wheels #26

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
5 changes: 3 additions & 2 deletions ca_driver/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
geometry_msgs
nav_msgs
sensor_msgs
tf
ca_msgs
)
Expand All @@ -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
Expand Down
4 changes: 4 additions & 0 deletions ca_driver/include/create_driver/create_driver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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_;
Expand All @@ -62,6 +64,7 @@ class CreateDriver

bool update();
void publishOdom();
void publishJointState();
void publishBatteryInfo();
void publishButtonPresses() const;
void publishOmniChar();
Expand Down Expand Up @@ -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);
Expand Down
2 changes: 2 additions & 0 deletions ca_driver/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
20 changes: 20 additions & 0 deletions ca_driver/src/create_driver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should we populate header.frame_id too? I'm not familiar with joint_state_publisher

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

joint_state_publisher doesn't do it, so I don't think it's necessary.

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++)
Expand Down Expand Up @@ -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.");
}

Expand Down Expand Up @@ -190,6 +197,7 @@ void CreateDriver::undockCallback(const std_msgs::EmptyConstPtr& msg)
bool CreateDriver::update()
{
publishOdom();
publishJointState();
publishBatteryInfo();
publishButtonPresses();
publishOmniChar();
Expand Down Expand Up @@ -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();
Expand Down