Go to the documentation of this file.00001 #include <kobuki_softnode/fake_kobuki_ros.h>
00002 #include <tf/transform_datatypes.h>
00003
00004 namespace kobuki
00005 {
00006 FakeKobukiRos::FakeKobukiRos(std::string& node_name)
00007 {
00008 this->name = node_name;
00009 }
00010
00011 FakeKobukiRos::~FakeKobukiRos()
00012 {
00013 }
00014
00015 bool FakeKobukiRos::init(ros::NodeHandle& nh)
00016 {
00017 kobuki.init(nh);
00018
00019
00020 advertiseTopics(nh);
00021
00022
00023 subscribeTopics(nh);
00024
00025 publishVersionInfoOnce();
00026
00027 this->prev_update_time = ros::Time::now();
00028 return true;
00029 }
00030
00031
00032 void FakeKobukiRos::advertiseTopics(ros::NodeHandle& nh)
00033 {
00034
00035 this->publisher["joint_states"] = nh.advertise<sensor_msgs::JointState>("joint_states",100);
00036
00037
00038 this->publisher["version_info"] = nh.advertise<kobuki_msgs::VersionInfo>("version_info",100,true);
00039
00040
00041 this->publisher["odom"] = nh.advertise<nav_msgs::Odometry>("odom",100);
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 }
00062
00063 void FakeKobukiRos::subscribeTopics(ros::NodeHandle& nh)
00064 {
00065 std::string cmd = "commands/";
00066 this->subscriber["velocity"] = nh.subscribe(cmd + "velocity", 10, &FakeKobukiRos::subscribeVelocityCommand, this);
00067 this->subscriber["motor_power"] = nh.subscribe(cmd + "motor_power", 10, &FakeKobukiRos::subscribeMotorPowerCommand,this);
00068 }
00069
00070 void FakeKobukiRos::publishVersionInfoOnce()
00071 {
00072 this->publisher["version_info"].publish(this->kobuki.versioninfo);
00073 }
00074
00075 void FakeKobukiRos::subscribeVelocityCommand(const geometry_msgs::TwistConstPtr msg)
00076 {
00077 this->last_cmd_vel_time = ros::Time::now();
00078 this->kobuki.wheel_speed_cmd[LEFT] = msg->linear.x - msg->angular.z * this->kobuki.wheel_separation / 2;
00079 this->kobuki.wheel_speed_cmd[RIGHT] = msg->linear.x + msg->angular.z * this->kobuki.wheel_separation / 2;
00080 }
00081
00082 void FakeKobukiRos::subscribeMotorPowerCommand(const kobuki_msgs::MotorPowerConstPtr msg)
00083 {
00084 if((msg->state == kobuki_msgs::MotorPower::ON) && (!this->kobuki.motor_enabled))
00085 {
00086 this->kobuki.motor_enabled = true;
00087 ROS_INFO_STREAM("Motors fire up. [" << this->name << "]");
00088 }
00089 else if((msg->state == kobuki_msgs::MotorPower::OFF) && (this->kobuki.motor_enabled))
00090 {
00091 this->kobuki.motor_enabled = false;
00092 ROS_INFO_STREAM("Motors take a break. [" << this->name << "]");
00093 }
00094 }
00095
00096 void FakeKobukiRos::updateJoint(unsigned int index,double& w,ros::Duration step_time)
00097 {
00098 double v;
00099 v = this->kobuki.wheel_speed_cmd[index];
00100 w = v / (this->kobuki.wheel_diameter / 2);
00101 this->kobuki.joint_states.velocity[index] = w;
00102 this->kobuki.joint_states.position[index]= this->kobuki.joint_states.position[index] + w * step_time.toSec();
00103 }
00104
00105 void FakeKobukiRos::updateOdometry(double w_left,double w_right,ros::Duration step_time)
00106 {
00107 double d1,d2;
00108 double dr,da;
00109 d1 = d2 = 0;
00110 dr = da = 0;
00111
00112 d1 = step_time.toSec() * (this->kobuki.wheel_diameter / 2) * w_left;
00113 d2 = step_time.toSec() * (this->kobuki.wheel_diameter / 2) * w_right;
00114
00115 if(isnan(d1))
00116 {
00117 d1 = 0;
00118 }
00119 if(isnan(d2))
00120 {
00121 d2 = 0;
00122 }
00123
00124 dr = (d1 + d2) / 2;
00125 da = (d2 - d1) / this->kobuki.wheel_separation;
00126
00127
00128 this->kobuki.odom_pose[0] += dr * cos(this->kobuki.odom_pose[2]);
00129 this->kobuki.odom_pose[1] += dr * sin(this->kobuki.odom_pose[2]);
00130 this->kobuki.odom_pose[2] += da;
00131
00132
00133 this->kobuki.odom_vel[0] = dr / step_time.toSec();
00134 this->kobuki.odom_vel[1] = 0.0;
00135 this->kobuki.odom_vel[2] = da / step_time.toSec();
00136
00137 this->kobuki.odom.pose.pose.position.x = this->kobuki.odom_pose[0];
00138 this->kobuki.odom.pose.pose.position.y = this->kobuki.odom_pose[1];
00139 this->kobuki.odom.pose.pose.position.z = 0;
00140 this->kobuki.odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(this->kobuki.odom_pose[2]);
00141 }
00142
00143 void FakeKobukiRos::updateTF(geometry_msgs::TransformStamped& odom_tf)
00144 {
00145 odom_tf.header = this->kobuki.odom.header;
00146 odom_tf.child_frame_id = this->kobuki.odom.child_frame_id;
00147 odom_tf.transform.translation.x = this->kobuki.odom.pose.pose.position.x;
00148 odom_tf.transform.translation.y = this->kobuki.odom.pose.pose.position.y;
00149 odom_tf.transform.translation.z = this->kobuki.odom.pose.pose.position.z;
00150 odom_tf.transform.rotation = this->kobuki.odom.pose.pose.orientation;
00151 }
00152
00153
00154 bool FakeKobukiRos::update()
00155 {
00156 ros::Time time_now = ros::Time::now();
00157 ros::Duration step_time = time_now - this->prev_update_time;
00158 this->prev_update_time = time_now;
00159
00160
00161 if(((time_now - this->last_cmd_vel_time).toSec() > this->kobuki.cmd_vel_timeout) || !this->kobuki.motor_enabled)
00162 {
00163 this->kobuki.wheel_speed_cmd[LEFT] = 0.0;
00164 this->kobuki.wheel_speed_cmd[RIGHT] = 0.0;
00165 }
00166
00167
00168 double w_left,w_right;
00169 updateJoint(LEFT,w_left,step_time);
00170 updateJoint(RIGHT,w_right,step_time);
00171 this->kobuki.joint_states.header.stamp = time_now;
00172 this->publisher["joint_states"].publish(this->kobuki.joint_states);
00173
00174
00175 updateOdometry(w_left,w_right,step_time);
00176 this->kobuki.odom.header.stamp = time_now;
00177 this->publisher["odom"].publish(this->kobuki.odom);
00178
00179
00180 geometry_msgs::TransformStamped odom_tf;
00181 updateTF(odom_tf);
00182 this->tf_broadcaster.sendTransform(odom_tf);
00183
00184 return true;
00185 }
00186 }