vehicle_static_tf.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2009 Austin Robot Technology
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  * 
00006  *  $Id: vehicle_static_tf.cc 1774 2011-09-10 19:23:37Z austinrobot $
00007  */
00008 
00009 #include <ros/ros.h>
00010 #include <angles/angles.h>
00011 #include <tf/transform_broadcaster.h>
00012 
00013 #include <art/frames.h>
00014 #include <art_msgs/ArtHertz.h>
00015 #include <art_msgs/ArtVehicle.h>
00016 
00034 #define NODE "vehicle_static_tf"
00035 
00036 namespace
00037 {
00039   //  into the future.  Otherwise, some transform listeners will see
00040   //  old data at times.
00041   ros::Duration transform_post_date_(1.0/art_msgs::ArtHertz::VEHICLE_TF);
00042 
00043   // class for generating vehicle-relative frame IDs
00044   ArtFrames::VehicleRelative vr_;
00045 }
00046 
00048 void broadcastTF(tf::TransformBroadcaster *tf_broadcaster,
00049                  std::string device_frame,
00050                  double x, double y, double z, 
00051                  double roll, double pitch, double yaw)
00052 {
00053   // translate roll, pitch and yaw into a Quaternion
00054   tf::Quaternion q;
00055   q.setRPY(roll, pitch, yaw);
00056   geometry_msgs::Quaternion tf_quat;
00057   tf::quaternionTFToMsg(q, tf_quat);
00058 
00059   // broadcast Transform from vehicle to device
00060   geometry_msgs::TransformStamped static_tf;
00061   static_tf.header.stamp = ros::Time::now() + transform_post_date_;
00062   static_tf.header.frame_id = vr_.getFrame(ArtFrames::vehicle);
00063   static_tf.child_frame_id = vr_.getFrame(device_frame);
00064   static_tf.transform.translation.x = x;
00065   static_tf.transform.translation.y = y;
00066   static_tf.transform.translation.z = z;
00067   static_tf.transform.rotation = tf_quat;
00068 
00069   tf_broadcaster->sendTransform(static_tf);
00070 }
00071 
00074 void opticalTF(tf::TransformBroadcaster *tf_broadcaster,
00075                std::string device_frame,
00076                double x, double y, double z, 
00077                double roll, double pitch, double yaw)
00078 {
00079   // first broadcast the device frame
00080   broadcastTF(tf_broadcaster, device_frame, x, y, z, roll, pitch, yaw);
00081 
00082   // broadcast Transform from device to corresponding optical frame
00083   geometry_msgs::TransformStamped optical_tf;
00084   optical_tf.header.stamp = ros::Time::now() + transform_post_date_;
00085   optical_tf.header.frame_id = vr_.getFrame(device_frame);
00086   optical_tf.child_frame_id = vr_.getFrame(device_frame + "_optical");
00087 
00088   // this Quaternion rotates the device frame into the optical frame
00089   optical_tf.transform.rotation.w = 0.5;
00090   optical_tf.transform.rotation.x = -0.5;
00091   optical_tf.transform.rotation.y = 0.5;
00092   optical_tf.transform.rotation.z = -0.5;
00093 
00094   tf_broadcaster->sendTransform(optical_tf);
00095 }
00096 
00098 int main(int argc, char** argv)
00099 {
00100   ros::init(argc, argv, NODE);
00101   ros::NodeHandle node;
00102 
00103   tf::TransformBroadcaster tf_broadcaster;
00104   vr_.getPrefixParam();                 // get vehicle-relative tf prefix
00105 
00106   ros::Rate cycle(art_msgs::ArtHertz::VEHICLE_TF); // set driver cycle rate
00107   
00108   ROS_INFO(NODE ": starting main loop");
00109 
00110   // main loop
00111   while(ros::ok())
00112     {
00113       using art_msgs::ArtVehicle;
00114 
00115       // Velodyne 3D LIDAR
00116       broadcastTF(&tf_broadcaster, ArtFrames::velodyne,
00117                   ArtVehicle::velodyne_px,
00118                   ArtVehicle::velodyne_py,
00119                   ArtVehicle::velodyne_pz,
00120                   ArtVehicle::velodyne_roll,
00121                   ArtVehicle::velodyne_pitch,
00122                   ArtVehicle::velodyne_yaw);
00123 
00124       // Front Sick LIDAR
00125       broadcastTF(&tf_broadcaster, ArtFrames::front_sick,
00126                   ArtVehicle::front_SICK_px,
00127                   ArtVehicle::front_SICK_py,
00128                   ArtVehicle::front_SICK_pz,
00129                   ArtVehicle::front_SICK_roll,
00130                   ArtVehicle::front_SICK_pitch,
00131                   ArtVehicle::front_SICK_yaw);
00132 
00133       // Rear Sick LIDAR
00134       broadcastTF(&tf_broadcaster, ArtFrames::rear_sick,
00135                   ArtVehicle::rear_SICK_px,
00136                   ArtVehicle::rear_SICK_py,
00137                   ArtVehicle::rear_SICK_pz,
00138                   ArtVehicle::rear_SICK_roll,
00139                   ArtVehicle::rear_SICK_pitch,
00140                   ArtVehicle::rear_SICK_yaw);
00141 
00142       // Left front camera
00143       opticalTF(&tf_broadcaster, ArtFrames::left_front_camera,
00144                   ArtVehicle::left_front_camera_px,
00145                   ArtVehicle::left_front_camera_py,
00146                   ArtVehicle::left_front_camera_pz,
00147                   ArtVehicle::left_front_camera_roll,
00148                   ArtVehicle::left_front_camera_pitch,
00149                   ArtVehicle::left_front_camera_yaw);
00150 
00151       // Center front camera
00152       opticalTF(&tf_broadcaster, ArtFrames::center_front_camera,
00153                   ArtVehicle::center_front_camera_px,
00154                   ArtVehicle::center_front_camera_py,
00155                   ArtVehicle::center_front_camera_pz,
00156                   ArtVehicle::center_front_camera_roll,
00157                   ArtVehicle::center_front_camera_pitch,
00158                   ArtVehicle::center_front_camera_yaw);
00159 
00160       // Right front camera
00161       opticalTF(&tf_broadcaster, ArtFrames::right_front_camera,
00162                   ArtVehicle::right_front_camera_px,
00163                   ArtVehicle::right_front_camera_py,
00164                   ArtVehicle::right_front_camera_pz,
00165                   ArtVehicle::right_front_camera_roll,
00166                   ArtVehicle::right_front_camera_pitch,
00167                   ArtVehicle::right_front_camera_yaw);
00168 
00169       ros::spinOnce();                  // handle incoming messages
00170       cycle.sleep();                    // sleep until next cycle
00171     }
00172 
00173   ROS_INFO(NODE ": exiting main loop");
00174 
00175   return 0;
00176 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


art_common
Author(s): Austin Robot Technology
autogenerated on Tue Sep 24 2013 10:41:19