base_motor_node.cpp
Go to the documentation of this file.
00001 
00031 #include "base_motor_node.h"
00032 
00034 
00035 BaseMotorNode::BaseMotorNode() :
00036     _nh_private("~"), _publish_rate(10)
00037 {
00038     _bm.init_communication();
00039 }
00040 
00042 
00043 BaseMotorNode::~BaseMotorNode()
00044 {
00045     _bm.end_communication();
00046 }
00047 
00049 
00050 void BaseMotorNode::init()
00051 {
00052     odom_pub = _nh.advertise<nav_msgs::Odometry>("odom", 50);
00053     sub = _nh.subscribe("cmd_vel", 1, &BaseMotorNode::twist_callback, this);
00054 }
00055 
00057 
00058 void BaseMotorNode::spin()
00059 {
00060     ROS_DEBUG("[BASE_MOTOR_NODE] wait for /clock to emit...");
00061     while(_last_time.isZero()) {
00062         _current_time = ros::Time::now();
00063         _last_time = ros::Time::now();
00064     }
00065     ROS_DEBUG("[BASE_MOTOR_NODE] /clock time received OK.");
00066 
00067     while(_nh.ok()) {
00068         ros::spinOnce();
00069         _publish_rate.sleep();
00070 
00071         publish();
00072     }
00073 }
00074 
00076 
00077 void BaseMotorNode::publish()
00078 {
00079     double x = 0, y = 0, th = 0;
00080 
00081     _current_time = ros::Time::now();
00082 
00083     // compute odometry in a typical way given the velocities of the robot
00084     double dt = (_current_time - _last_time).toSec();
00085 
00086     cinematic_data data;
00087     _bm.read_data_variable_time_diff(&data, dt);
00088 
00089     // speeds
00090     double vx = data.v / 1000.f; // mm/ sec -> m /sec
00091     double vy = 0; // the robot cannot have side speed
00092     double vth = data.w * M_PI / 180.f; // degrees /sec -> rad /sec
00093 
00094     // odometry
00095 
00096     // first method: trust the base primitive for odometry
00097     x = data.x / 1000.f; // mm -> m
00098     y = data.y / 1000.f; // mm -> m
00099     th = data.theta * M_PI / 180.f; // degrees -> rad
00100 
00101     // second method: first order integration
00102     // double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
00103     // double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
00104     // double delta_th = vth * dt;
00105     // x += delta_x;
00106     // y += delta_y;
00107     // th += delta_th;
00108 
00109     // since all odometry is 6DOF we'll need a quaternion created from yaw
00110     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
00111 
00112     // first, we'll publish the transform over tf
00113     geometry_msgs::TransformStamped odom_trans;
00114     odom_trans.header.stamp = _current_time;
00115     odom_trans.header.frame_id = "/odom";
00116     odom_trans.child_frame_id = "/base_link";
00117 
00118     odom_trans.transform.translation.x = x;
00119     odom_trans.transform.translation.y = y;
00120     odom_trans.transform.translation.z = 0.0;
00121     odom_trans.transform.rotation = odom_quat;
00122 
00123     // send the transform
00124     odom_broadcaster.sendTransform(odom_trans);
00125 
00126     // next, we'll publish the odometry message over ROS
00127     nav_msgs::Odometry odom;
00128     odom.header.stamp = _current_time;
00129     odom.header.frame_id = "/odom";
00130 
00131     // set the position
00132     std::ostringstream ans_position;
00133     ans_position << "(" << x << ", " << y << ", " << 0.0 << ")";
00134     std::ostringstream ans_orientation;
00135     ans_orientation << "(" << odom_quat.x << ", " << odom_quat.y << ", " << odom_quat.z << ")";
00136 
00137     odom.pose.pose.position.x = x;
00138     odom.pose.pose.position.y = y;
00139     odom.pose.pose.position.z = 0.0;
00140     odom.pose.pose.orientation = odom_quat;
00141 
00142     // set the velocity
00143     odom.child_frame_id = "/base_link";
00144     odom.twist.twist.linear.x = vx;
00145     odom.twist.twist.linear.y = vy;
00146     odom.twist.twist.angular.z = vth;
00147 
00148     // print mgs
00149     ROS_DEBUG_NAMED(
00150         "base_motor_node",
00151         "[BASE_MOTOR_NODE] _current_time:%g (dt:%g) - position: %s, orientation :%s (theta:%g) vx:%g, vy:%g, vth:%g",
00152         _current_time.toSec(), dt, ans_position.str().c_str(), ans_orientation.str().c_str(), th, vx, vy, vth);
00153 
00154     // publish the message
00155     odom_pub.publish(odom);
00156 
00157     _last_time = _current_time;
00158 }
00159 
00161 
00162 void BaseMotorNode::twist_callback(const geometry_msgs::Twist::ConstPtr& cmd_vel)
00163 {
00164     ROS_DEBUG_NAMED("base_motor_node", "[BASE_MOTOR_NODE] Linear (vx: %f, vy: %f, vz: %f)", cmd_vel->linear.x,
00165                     cmd_vel->linear.y, cmd_vel->linear.z);
00166     ROS_DEBUG_NAMED("base_motor_node", "[BASE_MOTOR_NODE] Angular (vx: %f, vy: %f, vtheta: %f)", cmd_vel->angular.x,
00167                     cmd_vel->angular.y, cmd_vel->angular.z);
00168 
00169     // convert linear speed: m/s -> mm/s
00170     double linear_speed_mm_s = cmd_vel->linear.x * 1000.f;
00171 
00172     // convert angular speed: radian/s -> degrees/s
00173     double angular_speed_deg_s = cmd_vel->angular.z * 180.f / M_PI;
00174 
00175     // set the new speeds
00176     _bm.set_velocity(linear_speed_mm_s, angular_speed_deg_s);
00177 }
00178 


base
Author(s): Raul Perula-Martinez
autogenerated on Wed Apr 1 2015 10:17:15