Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031 #include <geometry_msgs/Twist.h>
00032 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00033 #include <nav_msgs/Odometry.h>
00034 #include <tf2/utils.h>
00035 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00036 #include <tf2_ros/transform_broadcaster.h>
00037 #include <tf2_ros/transform_listener.h>
00038
00039 #include <neonavigation_common/compatibility.h>
00040
00041 class DummyRobotNode
00042 {
00043 protected:
00044 ros::NodeHandle nh_;
00045 ros::NodeHandle pnh_;
00046
00047 double x_;
00048 double y_;
00049 double yaw_;
00050 float v_;
00051 float w_;
00052
00053 ros::Publisher pub_odom_;
00054 ros::Subscriber sub_twist_;
00055 ros::Subscriber sub_init_;
00056 tf2_ros::Buffer tfbuf_;
00057 tf2_ros::TransformBroadcaster tfb_;
00058 tf2_ros::TransformListener tfl_;
00059
00060 void cbTwist(const geometry_msgs::Twist::ConstPtr& msg)
00061 {
00062 v_ = msg->linear.x;
00063 w_ = msg->angular.z;
00064 }
00065 void cbInit(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
00066 {
00067 geometry_msgs::PoseStamped pose_in, pose_out;
00068 pose_in.header = msg->header;
00069 pose_in.pose = msg->pose.pose;
00070 try
00071 {
00072 geometry_msgs::TransformStamped trans =
00073 tfbuf_.lookupTransform("odom", pose_in.header.frame_id, pose_in.header.stamp, ros::Duration(1.0));
00074 tf2::doTransform(pose_in, pose_out, trans);
00075 }
00076 catch (tf2::TransformException& e)
00077 {
00078 ROS_WARN("%s", e.what());
00079 return;
00080 }
00081
00082 x_ = pose_out.pose.position.x;
00083 y_ = pose_out.pose.position.y;
00084 yaw_ = tf2::getYaw(pose_out.pose.orientation);
00085 }
00086
00087 public:
00088 DummyRobotNode()
00089 : nh_()
00090 , pnh_("~")
00091 , tfl_(tfbuf_)
00092 {
00093 neonavigation_common::compat::checkCompatMode();
00094 pnh_.param("initial_x", x_, 0.0);
00095 pnh_.param("initial_y", y_, 0.0);
00096 pnh_.param("initial_yaw", yaw_, 0.0);
00097 v_ = 0.0;
00098 w_ = 0.0;
00099
00100 pub_odom_ = nh_.advertise<nav_msgs::Odometry>("odom", 1, true);
00101 sub_twist_ = nh_.subscribe("cmd_vel", 1, &DummyRobotNode::cbTwist, this);
00102 sub_init_ = nh_.subscribe("initialpose", 1, &DummyRobotNode::cbInit, this);
00103 }
00104 void spin()
00105 {
00106 const float dt = 0.01;
00107 ros::Rate rate(1.0 / dt);
00108
00109 while (ros::ok())
00110 {
00111 ros::spinOnce();
00112 rate.sleep();
00113 const ros::Time current_time = ros::Time::now();
00114
00115 yaw_ += w_ * dt;
00116 x_ += cosf(yaw_) * v_ * dt;
00117 y_ += sinf(yaw_) * v_ * dt;
00118
00119 geometry_msgs::TransformStamped trans;
00120 trans.header.stamp = current_time;
00121 trans.header.frame_id = "odom";
00122 trans.child_frame_id = "base_link";
00123 trans.transform.translation = tf2::toMsg(tf2::Vector3(x_, y_, 0.0));
00124 trans.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw_));
00125 tfb_.sendTransform(trans);
00126
00127 nav_msgs::Odometry odom;
00128 odom.header.frame_id = "odom";
00129 odom.header.stamp = current_time;
00130 odom.child_frame_id = "base_link";
00131 odom.pose.pose.position.x = x_;
00132 odom.pose.pose.position.y = y_;
00133 odom.pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw_));
00134 odom.twist.twist.linear.x = v_;
00135 odom.twist.twist.angular.z = w_;
00136 pub_odom_.publish(odom);
00137 }
00138 }
00139 };
00140
00141 int main(int argc, char* argv[])
00142 {
00143 ros::init(argc, argv, "dummy_robot");
00144
00145 DummyRobotNode robot;
00146 robot.spin();
00147
00148 return 0;
00149 }