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