Go to the documentation of this file.00001 #include "segway_rmp400_odom_6d_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
00047
00048 double dt = (current_time - last_time).toSec();
00049
00050
00051 segway_status_mutex_.enter();
00052 double vT = (left_wheels_velocity_ + right_wheels_velocity_) / 2;
00053 double vy = - yaw_rate_;
00054 double vr = roll_rate_;
00055 double vp = pitch_rate_;
00056 segway_status_mutex_.exit();
00057
00058
00059 r_ += vr * dt/2;
00060 p_ += vp * dt/2;
00061 y_ += vy * dt/2;
00062
00063
00064 double vTx = vT * cos(y_) * cos(p_);
00065 double vTy = vT * sin(y_) * cos(r_);
00066 double vTz = vT * sin(r_) * sin(p_);
00067
00068
00069 double dx = vTx * dt;
00070 double dy = vTy * dt;
00071 double dz = vTz * dt;
00072
00073
00074 r_ += vr * dt/2;
00075 p_ += vp * dt/2;
00076 y_ += vy * dt/2;
00077
00078
00079 geometry_msgs::Quaternion rot = tf::createQuaternionMsgFromRollPitchYaw (r_,p_,y_);
00080 transform_.translation.x += dx;
00081 transform_.translation.y += dy;
00082 transform_.translation.z += dz;
00083 transform_.rotation = rot;
00084
00085 pose_.pose.position.x += dx;
00086 pose_.pose.position.y += dy;
00087 pose_.pose.position.z += dz;
00088 pose_.pose.orientation = rot;
00089
00090 pose_.covariance[0] =
00091 pose_.covariance[7] = 0.01;
00092 pose_.covariance[14] =
00093 pose_.covariance[21] =
00094 pose_.covariance[28] =
00095 pose_.covariance[35] = 1e10;
00096
00097 twist_.twist.linear.x = vT;
00098 twist_.twist.linear.y = 0.0;
00099 twist_.twist.linear.z = 0.0;
00100 twist_.twist.angular.x = vr;
00101 twist_.twist.angular.y = vp;
00102 twist_.twist.angular.z = vy;
00103
00104
00105
00106 twist_.covariance[0] = 0.0001;
00107 twist_.covariance[7] = 0.0001;
00108 twist_.covariance[14] =
00109 twist_.covariance[21] =
00110 twist_.covariance[28] =
00111
00112 twist_.covariance[35] = 999;
00113
00114
00115 last_time = current_time;
00116
00117
00118 if(publish_tf_)
00119 {
00120
00121 odom_trans_msg.header.stamp = current_time;
00122 odom_trans_msg.header.frame_id = odom_id_;
00123 odom_trans_msg.child_frame_id = base_link_id_;
00124 odom_trans_msg.transform = transform_;
00125
00126 odom_broadcaster_.sendTransform(odom_trans_msg);
00127 }
00128
00129
00130 Odometry_msg_.header.stamp = current_time;
00131 Odometry_msg_.header.frame_id = odom_id_;
00132 Odometry_msg_.child_frame_id = base_link_id_;
00133 Odometry_msg_.pose = pose_;
00134 Odometry_msg_.twist = twist_;
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144 odom_publisher_.publish(Odometry_msg_);
00145 }
00146
00147
00148 void SegwayRmp400OdomAlgNode::segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg)
00149 {
00150 ROS_DEBUG("SegwayRmp400OdomAlgNode::segway_status_callback: New Message Received");
00151
00152
00153
00154 segway_status_mutex_.enter();
00155
00156
00157 left_wheels_velocity_ = (msg->rmp200[0].left_wheel_velocity + msg->rmp200[1].left_wheel_velocity)/2;
00158 right_wheels_velocity_ = (msg->rmp200[0].right_wheel_velocity + msg->rmp200[1].right_wheel_velocity)/2;
00159 yaw_rate_ = (msg->rmp200[0].yaw_rate + msg->rmp200[1].yaw_rate)/2;
00160 pitch_rate_ = (msg->rmp200[0].pitch_rate + msg->rmp200[1].pitch_rate) / 2;
00161 roll_rate_ = (msg->rmp200[0].roll_rate + msg->rmp200[1].roll_rate ) / 2;
00162
00163
00164
00165 segway_status_mutex_.exit();
00166 }
00167
00168
00169
00170
00171
00172
00173
00174 void SegwayRmp400OdomAlgNode::node_config_update(Config &config, uint32_t level)
00175 {
00176 alg_.lock();
00177
00178 alg_.unlock();
00179 }
00180
00181 void SegwayRmp400OdomAlgNode::addNodeDiagnostics(void)
00182 {
00183 }
00184
00185
00186 int main(int argc,char *argv[])
00187 {
00188 return algorithm_base::main<SegwayRmp400OdomAlgNode>(argc, argv, "segway_rmp400_odom_alg_node");
00189 }