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(); }