fake_kobuki_ros.cpp
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     // initialize publishers
00020     advertiseTopics(nh);
00021 
00022     // initialize subscribers
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     // turtlebot required
00035     this->publisher["joint_states"]  = nh.advertise<sensor_msgs::JointState>("joint_states",100);
00036 
00037     // kobuki esoterics
00038     this->publisher["version_info"] = nh.advertise<kobuki_msgs::VersionInfo>("version_info",100,true);
00039 
00040     // odometry
00041     this->publisher["odom"] = nh.advertise<nav_msgs::Odometry>("odom",100);
00042 
00043 
00044     /*
00045     // event publishers
00046     std::string evt = "events/";
00047     event_publisher["button"]         = nh.advertise<kobuki_msgs::SensorState>        (evt + "button",        100);
00048     event_publisher["bumper"]         = nh.advertise<kobuki_msgs::BumperEvent>        (evt + "bumper",        100);
00049     event_publisher["cliff"]          = nh.advertise<kobuki_msgs::CliffEvent>         (evt + "cliff",         100);
00050     event_publisher["wheel_drop"]     = nh.advertise<kobuki_msgs::WheelDropEvent>     (evt + "wheel_drop",    100);
00051     event_publisher["power_system"]   = nh.advertise<kobuki_msgs::PowerSystemEvent>   (evt + "power_system",  100);
00052     event_publisher["digital_input"]  = nh.advertise<kobuki_msgs::DigitalInputEvent>  (evt + "digital_input", 100);
00053     event_publisher["robot_state"]    = nh.advertise<kobuki_msgs::RobotStateEvent>    (evt + "robot_state",   100,true); // latched
00054 
00055   // sensor publishers
00056     std::string sen = "sensors/";
00057     sensor_publisher["core"]  = nh.advertise<kobuki_msgs::SensorState> (sen + "core", 100); 
00058     sensor_publisher["dock_ir"]  = nh.advertise<kobuki_msgs::SensorState> (sen + "dock_ir", 100); 
00059     sensor_publisher["imu_data"]  = nh.advertise<kobuki_msgs::SensorState> (sen + "imu_data", 100); 
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     // compute odometric pose
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     // compute odometric instantaneouse velocity
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     // zero-ing after timeout
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     // joint_states
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     // odom
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     // tf
00180     geometry_msgs::TransformStamped odom_tf;
00181     updateTF(odom_tf);
00182     this->tf_broadcaster.sendTransform(odom_tf);
00183 
00184     return true;
00185   }
00186 }


kobuki_softnode
Author(s): Jihoon Lee
autogenerated on Thu Aug 27 2015 13:44:29