ljl16

publisher

Nov 23rd, 2011
113
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*********************************************************************
  2. *
  3. * Software License Agreement (BSD License)
  4. *
  5. * Copyright (c) 2009, Willow Garage, Inc.
  6. * All rights reserved.
  7. *
  8. * Redistribution and use in source and binary forms, with or without
  9. * modification, are permitted provided that the following conditions
  10. * are met:
  11. *
  12. * * Redistributions of source code must retain the above copyright
  13. * notice, this list of conditions and the following disclaimer.
  14. * * Redistributions in binary form must reproduce the above
  15. * copyright notice, this list of conditions and the following
  16. * disclaimer in the documentation and/or other materials provided
  17. * with the distribution.
  18. * * Neither the name of Willow Garage, Inc. nor the names of its
  19. * contributors may be used to endorse or promote products derived
  20. * from this software without specific prior written permission.
  21. *
  22. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  23. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  24. * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  25. * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  26. * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  27. * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  28. * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  29. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  30. * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  31. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  32. * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  33. * POSSIBILITY OF SUCH DAMAGE.
  34. *
  35. * Author: Eitan Marder-Eppstein
  36. *********************************************************************/
  37. #include <ros/ros.h>
  38. #include <tf/transform_broadcaster.h>
  39. #include <nav_msgs/Odometry.h>
  40.  
  41. int main(int argc, char** argv){
  42. ros::init(argc, argv, "odometry_publisher");
  43.  
  44. ros::NodeHandle n;
  45. ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  46. tf::TransformBroadcaster odom_broadcaster;
  47.  
  48. double x = 0.0;
  49. double y = 0.0;
  50. double th = 0.0;
  51.  
  52. double vx = 0.5;
  53. double vy = -0.0;
  54. double vth = 0.21;
  55.  
  56. ros::Time current_time, last_time;
  57. current_time = ros::Time::now();
  58. last_time = ros::Time::now();
  59.  
  60. ros::Rate r(10.0);
  61. while(n.ok()){
  62. current_time = ros::Time::now();
  63.  
  64. //compute odometry in a typical way given the velocities of the robot
  65. double dt = (current_time - last_time).toSec();
  66. double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
  67. double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
  68. double delta_th = vth * dt;
  69.  
  70. x += delta_x;
  71. y += delta_y;
  72. th += delta_th;
  73.  
  74. //since all odometry is 6DOF we'll need a quaternion created from yaw
  75. geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
  76.  
  77. //first, we'll publish the transform over tf
  78. geometry_msgs::TransformStamped odom_trans;
  79. odom_trans.header.stamp = current_time;
  80. odom_trans.header.frame_id = "odom";
  81. odom_trans.child_frame_id = "base_link";
  82.  
  83. odom_trans.transform.translation.x = x;
  84. odom_trans.transform.translation.y = y;
  85. odom_trans.transform.translation.z = 0.0;
  86. odom_trans.transform.rotation = odom_quat;
  87.  
  88. //send the transform
  89. odom_broadcaster.sendTransform(odom_trans);
  90.  
  91. //next, we'll publish the odometry message over ROS
  92. nav_msgs::Odometry odom;
  93. odom.header.stamp = current_time;
  94. odom.header.frame_id = "odom";
  95. odom.child_frame_id = "base_link";
  96.  
  97. //set the position
  98. odom.pose.pose.position.x = x;
  99. odom.pose.pose.position.y = y;
  100. odom.pose.pose.position.z = 0.0;
  101. odom.pose.pose.orientation = odom_quat;
  102.  
  103. //set the velocity
  104. odom.twist.twist.linear.x = vx;
  105. odom.twist.twist.linear.y = vy;
  106. odom.twist.twist.angular.z = vth;
  107.  
  108. // publish the message
  109. odom_pub.publish(odom);
  110.  
  111. last_time = current_time;
  112. r.sleep();
  113. }
  114. }
  115.  
RAW Paste Data