Go to the documentation of this file.00001 #include "segway_rmp400_odom_alg_node.h"
00002
00003 SegwayRmp400OdomAlgNode::SegwayRmp400OdomAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<SegwayRmp400OdomAlgorithm>()
00005 {
00006
00007 loop_rate_ = 100;
00008
00009 public_node_handle_.param<std::string>("tf_prefix", tf_prefix_, "");
00010 public_node_handle_.param<std::string>("odom_id", odom_id_, "odom" );
00011 public_node_handle_.param<std::string>("base_link_id", base_link_id_, "base_footprint" );
00012 public_node_handle_.param<bool> ("publish_tf", publish_tf_, true );
00013
00014
00015 if( tf_prefix_.size()!=0 )
00016 {
00017 odom_id_ = tf_prefix_ + "/" + odom_id_;
00018 base_link_id_ = tf_prefix_ + "/" + base_link_id_;
00019 }
00020
00021
00022 odom_publisher_ = public_node_handle_.advertise<nav_msgs::Odometry>("odom", 100);
00023
00024
00025 segway_status_subscriber_ = public_node_handle_.subscribe("segway_status", 100, &SegwayRmp400OdomAlgNode::segway_status_callback, this);
00026
00027
00028
00029
00030
00031
00032
00033
00034 }
00035
00036 SegwayRmp400OdomAlgNode::~SegwayRmp400OdomAlgNode(void)
00037 {
00038
00039 }
00040
00041 void SegwayRmp400OdomAlgNode::mainNodeThread(void)
00042 {
00043 ros::Time current_time = ros::Time::now();
00044 geometry_msgs::TransformStamped odom_trans_msg;
00045 static ros::Time last_time = ros::Time::now();
00046 static double accum_th=0;
00047
00048
00049 double dt = (current_time - last_time).toSec();
00050
00051
00052 segway_status_mutex_.enter();
00053 double vT = (left_wheels_velocity_ + right_wheels_velocity_) / 2;
00054 double vth = - yaw_rate_;
00055 segway_status_mutex_.exit();
00056
00057 accum_th += vth * dt/2;
00058 double vx = vT*cos(accum_th);
00059 double vy = vT*sin(accum_th);
00060
00061
00062 double xx = vx * dt;
00063 double yy = vy * dt;
00064
00065
00066 accum_th += vth * dt/2;
00067
00068
00069
00070 geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(accum_th);
00071
00072
00073 transform_.translation.x += xx;
00074 transform_.translation.y += yy;
00075 transform_.translation.z += 0.0;
00076 transform_.rotation = odom_quat;
00077
00078
00079 pose_.pose.position.x += xx;
00080 pose_.pose.position.y += yy;
00081 pose_.pose.position.z += 0.0;
00082 pose_.pose.orientation = odom_quat;
00083
00084 pose_.covariance[0] =
00085 pose_.covariance[7] =
00086 pose_.covariance[14] =
00087 pose_.covariance[21] =
00088 pose_.covariance[28] =
00089 pose_.covariance[35] = 0.5;
00090
00091
00092 twist_.twist.linear.x = vT;
00093 twist_.twist.linear.y = 0.0;
00094 twist_.twist.linear.z = 0.0;
00095 twist_.twist.angular.x = 0.0;
00096 twist_.twist.angular.y = 0.0;
00097 twist_.twist.angular.z = vth;
00098
00099
00100
00101 twist_.covariance[0] = pow(0.1*vx,2);
00102 twist_.covariance[7] = pow(0.1*vy,2);
00103 twist_.covariance[14] =
00104 twist_.covariance[21] =
00105 twist_.covariance[28] = 1;
00106
00107 twist_.covariance[35] = pow(0.5*vth,2);
00108
00109
00110 last_time = current_time;
00111
00112
00113 if(publish_tf_)
00114 {
00115
00116 odom_trans_msg.header.stamp = current_time;
00117 odom_trans_msg.header.frame_id = odom_id_;
00118 odom_trans_msg.child_frame_id = base_link_id_;
00119 odom_trans_msg.transform = transform_;
00120
00121 odom_broadcaster_.sendTransform(odom_trans_msg);
00122 }
00123
00124
00125 Odometry_msg_.header.stamp = current_time;
00126 Odometry_msg_.header.frame_id = odom_id_;
00127 Odometry_msg_.child_frame_id = base_link_id_;
00128 Odometry_msg_.pose = pose_;
00129 Odometry_msg_.twist = twist_;
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139 odom_publisher_.publish(Odometry_msg_);
00140 }
00141
00142
00143 void SegwayRmp400OdomAlgNode::segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg)
00144 {
00145 ROS_DEBUG("SegwayRmp400OdomAlgNode::segway_status_callback: New Message Received");
00146
00147
00148
00149 segway_status_mutex_.enter();
00150
00151
00152 left_wheels_velocity_ = (msg->rmp200[0].left_wheel_velocity + msg->rmp200[1].left_wheel_velocity)/2;
00153 right_wheels_velocity_ = (msg->rmp200[0].right_wheel_velocity + msg->rmp200[1].right_wheel_velocity)/2;
00154 yaw_rate_ = msg->rmp200[0].yaw_rate;
00155
00156
00157
00158 segway_status_mutex_.exit();
00159 }
00160
00161
00162
00163
00164
00165
00166
00167 void SegwayRmp400OdomAlgNode::node_config_update(Config &config, uint32_t level)
00168 {
00169 alg_.lock();
00170
00171 alg_.unlock();
00172 }
00173
00174 void SegwayRmp400OdomAlgNode::addNodeDiagnostics(void)
00175 {
00176 }
00177
00178
00179 int main(int argc,char *argv[])
00180 {
00181 return algorithm_base::main<SegwayRmp400OdomAlgNode>(argc, argv, "segway_rmp400_odom_alg_node");
00182 }