void UpdateMapFromScan(OccupancyGrid& occ_map, const sensor_msgs::LaserScan& scan_data)
{
ros::Time scan_start_time = scan_data.header.stamp;
ros::Duration scan_duration = ros::Duration().fromSec(scan_data.time_increment * scan_data.ranges.size());
ros::Time scan_end_time = scan_start_time + scan_duration;
tf::StampedTransform robot_start_pos;
tf::StampedTransform robot_end_pos;
try
{
static tf::TransformListener tf_listener;
//Block until a position is available at the time of the scan
tf_listener.waitForTransform(WORLD_FRAME, ROBOT_FRAME, scan_end_time + TIME_SHIFT, ros::Duration(1.0));
tf_listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, scan_start_time + TIME_SHIFT, robot_start_pos);
tf_listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, scan_end_time + TIME_SHIFT, robot_end_pos);
}
catch(tf::TransformException &ex)
{
ROS_ERROR("%s", ex.what());
return;
}
//iterate over each beam
for(int beam_idx = 0; beam_idx < scan_data.ranges.size(); beam_idx++)
{
double beam_range = scan_data.ranges[beam_idx];
double angle = scan_data.angle_min + scan_data.angle_increment*beam_idx;
if(_checkValidBeam(beam_idx, scan_data))
{
tf::Vector3 beam_start = tf::Vector3(0.0, 0.0, 0.0);
tf::Vector3 beam_end = _getBeamHitPos(beam_range, angle);
double ratio = beam_idx/double(scan_data.ranges.size() - 1);
tf::Transform robot_pos = interpolateTF(robot_start_pos, robot_end_pos, ratio);
_mapUpdateBeamHit(occ_map, robot_pos(beam_start), robot_pos(beam_end));
}
}
}
tf::Stamped<tf::Pose> getRobotPose(const door_msgs::Door& door, double dist)
{
Vector x_axis(1,0,0);
// get vector to center of frame
Vector frame_1(door.frame_p1.x, door.frame_p1.y, door.frame_p1.z);
Vector frame_2(door.frame_p2.x, door.frame_p2.y, door.frame_p2.z);
Vector frame_center = (frame_2 + frame_1)/2.0;
// get robot pos
Vector frame_normal = getFrameNormal(door);
Vector robot_pos = frame_center + (frame_normal * dist);
tf::Stamped<tf::Pose> robot_pose;
robot_pose.frame_id_ = door.header.frame_id;
robot_pose.stamp_ = door.header.stamp;
robot_pose.setOrigin( tf::Vector3(robot_pos(0), robot_pos(1), robot_pos(2)));
robot_pose.setRotation( tf::createQuaternionFromRPY(getVectorAngle(x_axis, frame_normal), 0, 0) );
return robot_pose;
}
void Goalkeeper::buildPotentialFields(int robot_number)
{
updateBallPos();
Vector robot_pos(robot[robot_number].getX(),y_pos_);
Vector ball_line(ball_pos_.getX(),robot[robot_number].getY());
if (isBallInRange())
potential_fields_.push_back(new AttractivePotentialField(ball_line, 15));
else
stayAtTheBoundary();
}
