void TransporterNode::publish_odom()
{
// store the current pose
prevPose.x = cur_x;
prevPose.y = cur_y;
prevPose.theta = cur_theta;
// calculate the new pose
double distance, angle;
trobot.getDisplacement(&distance, &angle);
cur_theta += angle;
cur_x += distance * cos(cur_theta);
cur_y += distance * sin(cur_theta);
ROS_DEBUG("PREVIOUS x = [%f], y = [%f], theta = [%f]\n", prevPose.x, prevPose.y, prevPose.theta);
ROS_DEBUG("CURRENT x = [%f], y = [%f], theta = [%f]\n", cur_x, cur_y, cur_theta);
current_time = ros::Time::now();
double dt = (current_time - last_time).toSec();
// convert rotation about z into quaternion
geometry_msgs::Quaternion odom_quat;
odom_quat.z = sin(cur_theta/2.0);
odom_quat.w = cos(cur_theta/2.0);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
//set the position (with respect to header.frame)
odom.pose.pose.position.x = cur_x;
odom.pose.pose.position.y = cur_y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.pose.covariance =
boost::array<double, 36>{{1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-3}};
//set the velocity (with respect to child_frame)
odom.twist.twist.linear.x = distance/dt;
odom.twist.twist.angular.z = angle/dt;
odom.twist.covariance =
boost::array<double, 36>{{1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-3}};
//publish the message
odom_pub.publish(odom);
// publish joint state in order to show the continuous joints (wheels) in tf
sensor_msgs::JointState js;
js.header.stamp=current_time;
js.name.resize(2);
js.position.resize(2);
js.name[0]="left_wheel_joint";
js.name[1]="right_wheel_joint";
js.position[0]=0;
js.position[1]=0;
js_pub.publish(js);
last_time = current_time;
} //void TransporterNode::publish_odom()
