vehicle_configurable_tf.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2011 Austin Robot Technology
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  * 
00006  *  $Id: vehicle_configurable_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 
00017 #include <dynamic_reconfigure/server.h>
00018 #include <art_common/CameraTransformConfig.h>
00019 
00039 #define NODE "vehicle_configurable_tf"
00040 
00041 namespace
00042 {
00044   //  into the future.  Otherwise, some transform listeners will see
00045   //  old data at times.
00046   ros::Duration transform_post_date_(1.0/art_msgs::ArtHertz::VEHICLE_TF);
00047 
00048   // class for generating vehicle-relative frame IDs
00049   ArtFrames::VehicleRelative vr_;
00050 
00051   art_common::CameraTransformConfig config_;
00052 }
00053 
00055 void broadcastTF(tf::TransformBroadcaster *tf_broadcaster,
00056                  std::string device_frame,
00057                  double x, double y, double z, 
00058                  double roll, double pitch, double yaw)
00059 {
00060   // translate roll, pitch and yaw into a Quaternion
00061   tf::Quaternion q;
00062   q.setRPY(roll, pitch, yaw);
00063   geometry_msgs::Quaternion tf_quat;
00064   tf::quaternionTFToMsg(q, tf_quat);
00065 
00066   // broadcast Transform from vehicle to device
00067   geometry_msgs::TransformStamped static_tf;
00068   static_tf.header.stamp = ros::Time::now() + transform_post_date_;
00069   static_tf.header.frame_id = vr_.getFrame(ArtFrames::vehicle);
00070   static_tf.child_frame_id = vr_.getFrame(device_frame);
00071   static_tf.transform.translation.x = x;
00072   static_tf.transform.translation.y = y;
00073   static_tf.transform.translation.z = z;
00074   static_tf.transform.rotation = tf_quat;
00075 
00076   tf_broadcaster->sendTransform(static_tf);
00077 }
00078 
00081 void opticalTF(tf::TransformBroadcaster *tf_broadcaster,
00082                std::string device_frame,
00083                double x, double y, double z, 
00084                double roll, double pitch, double yaw)
00085 {
00086   // first broadcast the device frame
00087   broadcastTF(tf_broadcaster, device_frame, x, y, z, roll, pitch, yaw);
00088 
00089   // broadcast Transform from device to corresponding optical frame
00090   geometry_msgs::TransformStamped optical_tf;
00091   optical_tf.header.stamp = ros::Time::now() + transform_post_date_;
00092   optical_tf.header.frame_id = vr_.getFrame(device_frame);
00093   optical_tf.child_frame_id = vr_.getFrame(device_frame + "_optical");
00094 
00095   // this Quaternion rotates the device frame into the optical frame
00096   optical_tf.transform.rotation.w = 0.5;
00097   optical_tf.transform.rotation.x = -0.5;
00098   optical_tf.transform.rotation.y = 0.5;
00099   optical_tf.transform.rotation.z = -0.5;
00100 
00101   tf_broadcaster->sendTransform(optical_tf);
00102 }
00103 
00104 void callback(art_common::CameraTransformConfig &config, uint32_t level) {
00105   config_ = config;
00106 }
00107 
00109 int main(int argc, char** argv)
00110 {
00111   ros::init(argc, argv, NODE);
00112   ros::NodeHandle node;
00113 
00114   tf::TransformBroadcaster tf_broadcaster;
00115   vr_.getPrefixParam();                 // get vehicle-relative tf prefix
00116 
00117   ros::Rate cycle(art_msgs::ArtHertz::VEHICLE_TF); // set driver cycle rate
00118   
00119   // Start the dynamic reconfigure server to set the transform values
00120   dynamic_reconfigure::Server<art_common::CameraTransformConfig> srv;
00121   dynamic_reconfigure::Server<art_common::CameraTransformConfig>::CallbackType f = boost::bind(&callback, _1, _2);
00122   srv.setCallback(f);
00123 
00124   ROS_INFO(NODE ": starting main loop");
00125 
00126   // main loop
00127   while(ros::ok())
00128     {
00129       using art_msgs::ArtVehicle;
00130 
00131       // Velodyne 3D LIDAR
00132       broadcastTF(&tf_broadcaster, ArtFrames::velodyne,
00133                   config_.velodyne_px,
00134                   config_.velodyne_py,
00135                   config_.velodyne_pz,
00136                   config_.velodyne_roll,
00137                   config_.velodyne_pitch,
00138                   config_.velodyne_yaw);
00139 
00140       // Front Sick LIDAR
00141       broadcastTF(&tf_broadcaster, ArtFrames::front_sick,
00142                   ArtVehicle::front_SICK_px,
00143                   ArtVehicle::front_SICK_py,
00144                   ArtVehicle::front_SICK_pz,
00145                   ArtVehicle::front_SICK_roll,
00146                   ArtVehicle::front_SICK_pitch,
00147                   ArtVehicle::front_SICK_yaw);
00148 
00149       // Rear Sick LIDAR
00150       broadcastTF(&tf_broadcaster, ArtFrames::rear_sick,
00151                   ArtVehicle::rear_SICK_px,
00152                   ArtVehicle::rear_SICK_py,
00153                   ArtVehicle::rear_SICK_pz,
00154                   ArtVehicle::rear_SICK_roll,
00155                   ArtVehicle::rear_SICK_pitch,
00156                   ArtVehicle::rear_SICK_yaw);
00157 
00158       // Left front camera
00159       opticalTF(&tf_broadcaster, ArtFrames::left_front_camera,
00160                   config_.left_front_camera_px,
00161                   config_.left_front_camera_py,
00162                   config_.left_front_camera_pz,
00163                   config_.left_front_camera_roll,
00164                   config_.left_front_camera_pitch,
00165                   config_.left_front_camera_yaw);
00166 
00167       // Center front camera
00168       opticalTF(&tf_broadcaster, ArtFrames::center_front_camera,
00169                   config_.center_front_camera_px,
00170                   config_.center_front_camera_py,
00171                   config_.center_front_camera_pz,
00172                   config_.center_front_camera_roll,
00173                   config_.center_front_camera_pitch,
00174                   config_.center_front_camera_yaw);
00175 
00176       // Right front camera
00177       opticalTF(&tf_broadcaster, ArtFrames::right_front_camera,
00178                   config_.right_front_camera_px,
00179                   config_.right_front_camera_py,
00180                   config_.right_front_camera_pz,
00181                   config_.right_front_camera_roll,
00182                   config_.right_front_camera_pitch,
00183                   config_.right_front_camera_yaw);
00184 
00185       ros::spinOnce();                  // handle incoming messages
00186       cycle.sleep();                    // sleep until next cycle
00187     }
00188 
00189   ROS_INFO(NODE ": exiting main loop");
00190 
00191   return 0;
00192 }
 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