nj_oa_costmap: twist_handler.cpp Source File

Go to the documentation of this file.
00001 #include <nj_oa_costmap/twist_handler.h>
00002 
00003 namespace nj_oa_costmap
00004 {
00005 
00006 TwistHandler::TwistHandler(const double robot_radius, const std::string& laser_frame) :
00007   nj_oa_laser::TwistHandler(robot_radius),
00008   laser_frame(laser_frame),
00009   fake_laser_beam_count(40),
00010   range_max(5)
00011 {
00012 }
00013 
00014 geometry_msgs::Twist TwistHandler::getTwist(const nav_msgs::OccupancyGrid& map)
00015 {
00016   // Get the rotation between map and the LaserScan messages that were
00017   // used to build the map.
00018   tf::StampedTransform tr;
00019   try
00020   {
00021     tf_listerner_.waitForTransform(laser_frame, map.header.frame_id,
00022         map.header.stamp, ros::Duration(0.2));
00023     tf_listerner_.lookupTransform(laser_frame, map.header.frame_id,
00024         map.header.stamp, tr);
00025   }
00026   catch (tf::TransformException ex)
00027   {
00028     ROS_ERROR("%s", ex.what());
00029   }
00030   const double theta = tf::getYaw(tr.getRotation());
00031 
00032   sensor_msgs::LaserScan scan;
00033   scan.angle_min = -M_PI_2 - theta;
00034   scan.angle_max = M_PI_2 - theta;
00035   scan.angle_increment = (scan.angle_max - scan.angle_min) / (fake_laser_beam_count - 1);
00036   scan.range_max = range_max;
00037   scan.header = map.header;
00038   scan.header.frame_id = laser_frame;
00039   ray_caster_.laserScanCast(map, scan);
00040   
00041   scan.angle_min += theta;
00042   scan.angle_max += theta;
00043 
00044   geometry_msgs::Twist twist = nj_oa_laser::TwistHandler::getTwist(scan);
00045   return twist;
00046 }
00047 
00048 } // namespace nj_costmap