bool
AmclThread::get_odom_pose(tf::Stamped<tf::Pose>& odom_pose, double& x,
double& y, double& yaw,
const fawkes::Time* t, const std::string& f)
{
// Get the robot's pose
tf::Stamped<tf::Pose>
ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
tf::Vector3(0, 0, 0)), t, f);
try {
tf_listener->transform_pose(odom_frame_id_, ident, odom_pose);
} catch (Exception &e) {
if (cfg_buffer_debug_) {
logger->log_warn(name(), "Failed to compute odom pose (%s)",
e.what_no_backtrace());
}
return false;
}
x = odom_pose.getOrigin().x();
y = odom_pose.getOrigin().y();
double pitch, roll;
odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);
//logger->log_info(name(), "Odom pose: (%f, %f, %f)", x, y, yaw);
return true;
}
void publishTransform(tf::Stamped<tf::Pose> _odom2map)
{
/*
double x, y, th;
x = odom2map.getOrigin()[0];
y = odom2map.getOrigin()[1];
th = tf::getYaw(odom2map.getRotation());
ROS_INFO("x = %f y = %f th = %f",
(x), (y), (th));
* */
tf::Transform tfom = tf::Transform(tf::Quaternion(_odom2map.getRotation()),
tf::Point(_odom2map.getOrigin()));
tf::StampedTransform map_trans_stamped(tfom.inverse(), current_time, mapFrame, odomFrame);
tf_broadcaster->sendTransform(map_trans_stamped);
/*
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped map_trans;
mtx.lock();
map_trans.header.stamp = current_time;
mtx.unlock();
map_trans.header.frame_id = mapFrame;
map_trans.child_frame_id = odomFrame;
map_trans.transform.translation.x = coords.x;
map_trans.transform.translation.y = coords.y;
map_trans.transform.translation.z = 0.0;
geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(coords.th);
map_trans.transform.rotation = quat;
//send the transform
tf_broadcaster->sendTransform(map_trans);
*/
}
void SlamGMapping::writeOdomMsgtoFile(tf::Stamped<tf::Transform>& odom_pose){
tf::Quaternion quat = odom_pose.getRotation();
fwrite(&quat,sizeof(tf::Quaternion),1,odomfile);
fwrite(&odom_pose.getOrigin(),sizeof(tf::Vector3),1,odomfile);
/*
tfScalar x = odom_pose.getRotation().getX();
tfScalar y = odom_pose.getRotation().getY();
tfScalar z = odom_pose.getRotation().getZ();
tfScalar w = odom_pose.getRotation().getW();
fwrite(&x,sizeof(tfScalar),1,odomfile);
fwrite(&y,sizeof(tfScalar),1,odomfile);
fwrite(&z,sizeof(tfScalar),1,odomfile);
fwrite(&w,sizeof(tfScalar),1,odomfile);
x = odom_pose.getOrigin().getX();
y = odom_pose.getOrigin().getY();
z = odom_pose.getOrigin().getZ();
fwrite(&x,sizeof(tfScalar),1,odomfile);
fwrite(&y,sizeof(tfScalar),1,odomfile);
fwrite(&z,sizeof(tfScalar),1,odomfile);*/
}
double ExploreFrontier::getOrientationChange(const Frontier& frontier, const tf::Stamped<tf::Pose>& robot_pose){
double robot_yaw = tf::getYaw(robot_pose.getRotation());
double robot_atan2 = atan2(robot_pose.getOrigin().y() + sin(robot_yaw), robot_pose.getOrigin().x() + cos(robot_yaw));
double frontier_atan2 = atan2(frontier.pose.position.x, frontier.pose.position.y);
double orientation_change = robot_atan2 - frontier_atan2;
ROS_DEBUG("Orientation change: %.3f", orientation_change * (180.0 / M_PI));
return orientation_change;
}
void SlamGMapping::writePoseStamped(tf::Stamped<tf::Pose> & pose_stamped){
//fwrite(&pose_stamped.frame_id_,sizeof();
tf::Quaternion quat = pose_stamped.getRotation();
fwrite(&quat,sizeof(tf::Quaternion),1,posestampedfile);
fwrite(&pose_stamped.getOrigin(),sizeof(tf::Vector3),1,posestampedfile);
}
// compute nav fn (using wavefront assumption)
void getNavFn(double* cost_map, tf::StampedTransform &robot_to_map_transform, double* nav_fn) {
Point robot_pt(robot_to_map_transform.getOrigin().getX() / MAP_RESOLUTION,robot_to_map_transform.getOrigin().getY() / MAP_RESOLUTION);
Point goal_pt(goal_pose.getOrigin().getX() / MAP_RESOLUTION, goal_pose.getOrigin().getY() / MAP_RESOLUTION);
ROS_INFO("[getNavFn]\trobot_pt: (%d,%d), goal_pt(%d,%d)",robot_pt.x,robot_pt.y,goal_pt.x,goal_pt.y);
// setup init map
bool init_set_map[MAP_CELLS];
for (int i=0; i<MAP_CELLS;i++) {
init_set_map[i] = false;
}
setVal(init_set_map,goal_pt.x,goal_pt.y,true);
//updateMapDijkstra(nav_fn, init_set_map, goal_pt, cost_map, INT_MAX);
LPN2(nav_fn, cost_map, goal_pt, robot_pt);
}
void EDWAPlanner::updatePlanAndLocalCosts(
tf::Stamped<tf::Pose> global_pose,
const std::vector<geometry_msgs::PoseStamped>& new_plan) {
global_plan_.resize(new_plan.size());
for (unsigned int i = 0; i < new_plan.size(); ++i) {
global_plan_[i] = new_plan[i];
}
// costs for going away from path
path_costs_.setTargetPoses(global_plan_);
// costs for not going towards the local goal as much as possible
goal_costs_.setTargetPoses(global_plan_);
// alignment costs
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
double sq_dist =
(pos[0] - goal_pose.pose.position.x) * (pos[0] - goal_pose.pose.position.x) +
(pos[1] - goal_pose.pose.position.y) * (pos[1] - goal_pose.pose.position.y);
// we want the robot nose to be drawn to its final position
// (before robot turns towards goal orientation), not the end of the
// path for the robot center. Choosing the final position after
// turning towards goal orientation causes instability when the
// robot needs to make a 180 degree turn at the end
std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
double angle_to_goal = atan2(goal_pose.pose.position.y - pos[1], goal_pose.pose.position.x - pos[0]);
front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x +
forward_point_distance_ * cos(angle_to_goal);
front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ *
sin(angle_to_goal);
goal_front_costs_.setTargetPoses(front_global_plan);
// keeping the nose on the path
if (sq_dist > forward_point_distance_ * forward_point_distance_ * cheat_factor_) {
double resolution = planner_util_->getCostmap()->getResolution();
alignment_costs_.setScale(resolution * pdist_scale_ * 0.5);
// costs for robot being aligned with path (nose on path, not ju
alignment_costs_.setTargetPoses(global_plan_);
} else {
// once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
alignment_costs_.setScale(0.0);
}
}
void publishMarker(const tf::Stamped<tf::Point>& target) {
//create marker object
visualization_msgs::Marker marker;
uint32_t shape = visualization_msgs::Marker::SPHERE;
// Set the frame ID and timestamp.
marker.header.frame_id = target.frame_id_;
marker.header.stamp = ros::Time::now();
marker.ns = "head_target";
marker.id = 0;
// Set the marker type.
marker.type = shape;
// Set the marker action.
marker.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker.
marker.pose.position.x = target.getX();
marker.pose.position.y = target.getY();
marker.pose.position.z = target.getZ();
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker.scale.x = 0.08;
marker.scale.y = 0.08;
marker.scale.z = 0.08;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = 1.0f;
marker.color.g = 0.0f;
marker.color.b = 1.0f;
marker.color.a = 0.6;
marker.lifetime = ros::Duration();
// Publish the marker
marker_pub_.publish(marker);
}
void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
ROS_ASSERT(global_plan.size() >= plan.size());
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
while(it != plan.end()){
const geometry_msgs::PoseStamped& w = *it;
// Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
double x_diff = global_pose.getOrigin().x() - w.pose.position.x;
double y_diff = global_pose.getOrigin().y() - w.pose.position.y;
double distance_sq = x_diff * x_diff + y_diff * y_diff;
if(distance_sq < 1){
ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
break;
}
it = plan.erase(it);
global_it = global_plan.erase(global_it);
}
}
int main(int argc, char** argv) {
// init node
ros::init(argc, argv, "planner_simple");
ros::NodeHandle nh;
// set up subscribers, publishers and listeners
ros::Subscriber goal_sub = nh.subscribe("/goal", 1000, goalCallback);
ros::Subscriber map_sub = nh.subscribe("/map", 1000, mapCallback);
//ros::Subscriber obstacle_map_sub = nh.subscribe("/obstacle_map", 1000, mapCallback);
marker_pub = nh.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
map_pub = nh.advertise<assn2::NavGrid>( "nav_grid", 0 );
tf::TransformListener listener;
listener.waitForTransform("/map", "/base_link", ros::Time(), ros::Duration(1.5));
// set initial goal to be the robot's pose
try {
tf::StampedTransform map_base_link_transform; //lookup transform only works for this type
listener.lookupTransform("/map", "/base_link", ros::Time(0), map_base_link_transform);
goal_pose.setOrigin(map_base_link_transform.getOrigin());
goal_pose.setRotation(map_base_link_transform.getRotation());
} catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}
ros::Rate rate(10);
while (ros::ok()) {
// populates the goal_pose and global_map if a new message has been published
ros::spinOnce();
double cost_map[MAP_CELLS];
getCostFn(cost_map);
visualizeHeatMap(cost_map,"intrinsic_cost_map");
double nav_fn[MAP_CELLS];
tf::StampedTransform robot_to_map_transform; // the pose of the robot in the map frame
tf_util::getTransform(listener,"/base_link","/map",robot_to_map_transform);
getNavFn(cost_map,robot_to_map_transform, nav_fn);
visualizeHeatMap(nav_fn,"nav_fn");
publishMap(nav_fn);
rate.sleep();
}
}
bool getGoalPose (const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose)
{
if (global_plan.empty())
{
ROS_ERROR ("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try
{
tf::StampedTransform transform;
tf.waitForTransform (global_frame, ros::Time::now(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, ros::Duration (0.5));
tf.lookupTransform (global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, transform);
poseStampedMsgToTF (plan_goal_pose, goal_pose);
goal_pose.setData (transform * goal_pose);
goal_pose.stamp_ = transform.stamp_;
goal_pose.frame_id_ = global_frame;
}
catch (tf::LookupException& ex)
{
ROS_ERROR ("No Transform available Error: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR ("Connectivity Error: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR ("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR ("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int) global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}
void OdometryHelperRos::getRobotVel(tf::Stamped<tf::Pose>& robot_vel) {
// Set current velocities from odometry
geometry_msgs::Twist global_vel;
{
boost::mutex::scoped_lock lock(odom_mutex_);
global_vel.linear.x = base_odom_.twist.twist.linear.x;
global_vel.linear.y = base_odom_.twist.twist.linear.y;
global_vel.angular.z = base_odom_.twist.twist.angular.z;
robot_vel.frame_id_ = base_odom_.child_frame_id;
}
robot_vel.setData(tf::Transform(tf::createQuaternionFromYaw(global_vel.angular.z), tf::Vector3(global_vel.linear.x, global_vel.linear.y, 0)));
robot_vel.stamp_ = ros::Time();
}
bool
AmclNode::getOdomPose(tf::Stamped<tf::Pose>& odom_pose,
double& x, double& y, double& yaw,
const ros::Time& t, const std::string& f)
{
// Get the robot's pose
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
tf::Vector3(0,0,0)), t, f);
try
{
this->tf_->transformPose(odom_frame_id_, ident, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
x = odom_pose.getOrigin().x();
y = odom_pose.getOrigin().y();
double pitch,roll;
odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);
return true;
}
void publishMap(double* map) {
assn2::NavGrid ng;
ng.width = (uint32_t) MAP_SIZE_X;
ng.height = (uint32_t) MAP_SIZE_Y;
ng.resolution = MAP_RESOLUTION;
//convert the map to a vector
std::vector<double> costs(ng.width*ng.height);
for(uint32_t i = 0; i < ng.width*ng.height; i++) { costs[i] = map[i]; }
ng.costs = costs;
ng.goalX = goal_pose.getOrigin().getX();
ng.goalY = goal_pose.getOrigin().getY();
//publish
map_pub.publish(ng);
}
/*
* given the current state of the robot, find a good trajectory
*/
base_local_planner::Trajectory DWAPlanner::findBestPath(
tf::Stamped<tf::Pose> global_pose,
tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities,
std::vector<geometry_msgs::Point> footprint_spec) {
obstacle_costs_.setFootprint(footprint_spec);
//make sure that our configuration doesn't change mid-run
boost::mutex::scoped_lock l(configuration_mutex_);
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
// prepare cost functions and generators for this run
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
result_traj_.cost_ = -7;
// find best trajectory by sampling and scoring the samples
scored_sampling_planner_.findBestTrajectory(result_traj_, NULL);
// verbose publishing of point clouds
if (publish_cost_grid_pc_) {
//we'll publish the visualization of the costs to rviz before returning our best trajectory
map_viz_.publishCostCloud(*(planner_util_->getCostmap()));
}
// debrief stateful scoring functions
oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel);
//if we don't have a legal trajectory, we'll just command zero
if (result_traj_.cost_ < 0) {
drive_velocities.setIdentity();
} else {
tf::Vector3 start(result_traj_.xv_, result_traj_.yv_, 0);
drive_velocities.setOrigin(start);
tf::Matrix3x3 matrix;
matrix.setRotation(tf::createQuaternionFromYaw(result_traj_.thetav_));
drive_velocities.setBasis(matrix);
}
return result_traj_;
}
//given the current state of the robot, find a good trajectory
base_local_planner::Trajectory DWAPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities){
//make sure that our configuration doesn't change mid-run
boost::mutex::scoped_lock l(configuration_mutex_);
//make sure to get an updated copy of the costmap before computing trajectories
costmap_ros_->getCostmapCopy(costmap_);
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
//reset the map for new operations
map_.resetPathDist();
front_map_.resetPathDist();
//make sure that we update our path based on the global plan and compute costs
map_.setPathCells(costmap_, global_plan_);
std::vector<geometry_msgs::PoseStamped> front_global_plan = global_plan_;
front_global_plan.back().pose.position.x = front_global_plan.back().pose.position.x + forward_point_distance_ * cos(tf::getYaw(front_global_plan.back().pose.orientation));
front_global_plan.back().pose.position.y = front_global_plan.back().pose.position.y + forward_point_distance_ * sin(tf::getYaw(front_global_plan.back().pose.orientation));
front_map_.setPathCells(costmap_, front_global_plan);
ROS_DEBUG_NAMED("dwa_local_planner", "Path/Goal distance computed");
//rollout trajectories and find the minimum cost one
base_local_planner::Trajectory best = computeTrajectories(pos, vel);
ROS_DEBUG_NAMED("dwa_local_planner", "Trajectories created");
//if we don't have a legal trajectory, we'll just command zero
if(best.cost_ < 0){
drive_velocities.setIdentity();
}
else{
tf::Vector3 start(best.xv_, best.yv_, 0);
drive_velocities.setOrigin(start);
tf::Matrix3x3 matrix;
matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
drive_velocities.setBasis(matrix);
}
//we'll publish the visualization of the costs to rviz before returning our best trajectory
map_viz_.publishCostCloud();
return best;
}
bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
global_pose.setIdentity();
tf::Stamped < tf::Pose > robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
//get the global pose of the robot
try
{
tf_.transformPose(global_frame_, robot_pose, global_pose);
}
catch (tf::LookupException& ex)
{
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
return false;
}
// check global_pose timeout
if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
{
ROS_WARN_THROTTLE(1.0,
"Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
return false;
}
return true;
}
bool BallPickerLayer::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
global_pose.setIdentity();
tf::Stamped <tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
try
{
tfl.transformPose(global_frame_, robot_pose, global_pose);
}
catch (tf::TransformException& ex)
{
ROS_ERROR_THROTTLE(1.0, "TF exception looking up robot pose: %s/n", ex.what());
return false;
}
return true;
}
//given the current state of the robot, find a good trajectory
Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities){
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
//reset the map for new operations
path_map_.resetPathDist();
goal_map_.resetPathDist();
//temporarily remove obstacles that are within the footprint of the robot
std::vector<base_local_planner::Position2DInt> footprint_list =
footprint_helper_.getFootprintCells(
pos,
footprint_spec_,
costmap_,
true);
//mark cells within the initial footprint of the robot
for (unsigned int i = 0; i < footprint_list.size(); ++i) {
path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
}
//make sure that we update our path based on the global plan and compute costs
path_map_.setTargetCells(costmap_, global_plan_);
goal_map_.setLocalGoal(costmap_, global_plan_);
ROS_DEBUG("Path/Goal distance computed");
//rollout trajectories and find the minimum cost one
Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
vel[0], vel[1], vel[2],
acc_lim_x_, acc_lim_y_, acc_lim_theta_);
ROS_DEBUG("Trajectories created");
/*
//If we want to print a ppm file to draw goal dist
char buf[4096];
sprintf(buf, "base_local_planner.ppm");
FILE *fp = fopen(buf, "w");
if(fp){
fprintf(fp, "P3\n");
fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
fprintf(fp, "255\n");
for(int j = map_.size_y_ - 1; j >= 0; --j){
for(unsigned int i = 0; i < map_.size_x_; ++i){
int g_dist = 255 - int(map_(i, j).goal_dist);
int p_dist = 255 - int(map_(i, j).path_dist);
if(g_dist < 0)
g_dist = 0;
if(p_dist < 0)
p_dist = 0;
fprintf(fp, "%d 0 %d ", g_dist, 0);
}
fprintf(fp, "\n");
}
fclose(fp);
}
*/
if(best.cost_ < 0){
drive_velocities.setIdentity();
}
else{
tf::Vector3 start(best.xv_, best.yv_, 0);
drive_velocities.setOrigin(start);
tf::Matrix3x3 matrix;
matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
drive_velocities.setBasis(matrix);
}
return best;
}
void SlamGMapping::writeLaserPoseStamped(tf::Stamped<tf::Transform>& laser_pose){
tf::Quaternion quat = laser_pose.getRotation();
fwrite(&quat,sizeof(tf::Quaternion),1,laserposefile);
fwrite(&laser_pose.getOrigin(),sizeof(tf::Vector3),1,laserposefile);
}
bool ExploreFrontier::getExplorationGoals(
tf::Stamped<tf::Pose> robot_pose,
std::vector<geometry_msgs::Pose>& goals,
double potential_scale, double orientation_scale, double gain_scale)
{
findFrontiers();
if (frontiers_.size() == 0) {
ROS_DEBUG("no frontiers found");
return false;
}
geometry_msgs::Point start;
start.x = robot_pose.getOrigin().x();
start.y = robot_pose.getOrigin().y();
start.z = robot_pose.getOrigin().z();
planner_->computePotential(start);
//we'll make sure that we set goals for the frontier at least the circumscribed
//radius away from unknown space
// 2015: this may not be necessary
// float step = -1.0 * costmap_resolution;
// int c = ceil(costmap.getCircumscribedRadius() / costmap_resolution);
// WeightedFrontier goal;
std::vector<WeightedFrontier> weightedFrontiers;
weightedFrontiers.reserve(frontiers_.size());
for (auto& frontier: frontiers_) {
WeightedFrontier weightedFrontier;
weightedFrontier.frontier = frontier;
tf::Point p(frontier.pose.position.x, frontier.pose.position.y, frontier.pose.position.z);
tf::Quaternion bt;
tf::quaternionMsgToTF(frontier.pose.orientation, bt);
tf::Vector3 v(cos(bt.getAngle()), sin(bt.getAngle()), 0.0);
// for (int j=0; j <= c; j++) {
// tf::Vector3 check_point = p + (v * (step * j));
// weightedFrontier.frontier.pose.position.x = check_point.x();
// weightedFrontier.frontier.pose.position.y = check_point.y();
// weightedFrontier.frontier.pose.position.z = check_point.z();
// weightedFrontier.cost = potential_scale * getFrontierCost(weightedFrontier.frontier) + orientation_scale * getOrientationChange(weightedFrontier.frontier, robot_pose) - gain_scale * getFrontierGain(weightedFrontier.frontier, costmapResolution_);
// // weightedFrontier.cost = getFrontierCost(weightedFrontier.frontier) - getFrontierGain(weightedFrontier.frontier, costmapResolution_);
// // ROS_DEBUG("cost: %f (%f * %f + %f * %f - %f * %f)",
// // weightedFrontier.cost,
// // potential_scale,
// // getFrontierCost(weightedFrontier.frontier),
// // orientation_scale,
// // getOrientationChange(weightedFrontier.frontier, robot_pose),
// // gain_scale,
// // getFrontierGain(weightedFrontier.frontier, costmapResolution_) );
// weightedFrontiers.push_back(weightedFrontier);
// }
weightedFrontier.cost =
potential_scale * getFrontierCost(weightedFrontier.frontier)
+ orientation_scale * getOrientationChange(weightedFrontier.frontier, robot_pose)
- gain_scale * getFrontierGain(weightedFrontier.frontier);
weightedFrontiers.push_back(std::move(weightedFrontier));
}
goals.clear();
goals.reserve(weightedFrontiers.size());
std::sort(weightedFrontiers.begin(), weightedFrontiers.end());
for (uint i = 0; i < weightedFrontiers.size(); i++) {
goals.push_back(weightedFrontiers[i].frontier.pose);
}
return (goals.size() > 0);
}
double getGoalPositionDistance(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y) {
return hypot(goal_x - global_pose.getOrigin().x(), goal_y - global_pose.getOrigin().y());
}
double getGoalOrientationAngleDifference(const tf::Stamped<tf::Pose>& global_pose, double goal_th) {
double yaw = tf::getYaw(global_pose.getRotation());
return angles::shortest_angular_distance(yaw, goal_th);
}
void SlamGMapping::writeMinMax(tf::Stamped<tf::Quaternion> & minmax){
tf::Quaternion quat (minmax.getX(),minmax.getY(),minmax.getZ(),minmax.getW());
fwrite(&quat,sizeof(tf::Quaternion),1,minmaxfile);
}
return true;
}
bool isGoalReached (const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const costmap_2d::Costmap2D& costmap __attribute__ ( (unused)),
const std::string& global_frame,
tf::Stamped<tf::Pose>& global_pose,
const nav_msgs::Odometry& base_odom,
double rot_stopped_vel, double trans_stopped_vel,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
//we assume the global goal is the last point in the global plan
tf::Stamped<tf::Pose> goal_pose;
getGoalPose (tf, global_plan, global_frame, goal_pose);
double goal_x = goal_pose.getOrigin().getX();
double goal_y = goal_pose.getOrigin().getY();
double goal_th = tf::getYaw (goal_pose.getRotation());
//check to see if we've reached the goal position
if (getGoalPositionDistance (global_pose, goal_x, goal_y) <= xy_goal_tolerance)
{
//check to see if the goal orientation has been reached
if (fabs (getGoalOrientationAngleDifference (global_pose, goal_th)) <= yaw_goal_tolerance)
{
//make sure that we're actually stopped before returning success
if (stopped (base_odom, rot_stopped_vel, trans_stopped_vel))
return true;
/*
* given the current state of the robot, find a good trajectory
*/
base_local_planner::Trajectory EDWAPlanner::findBestPath(
tf::Stamped<tf::Pose> global_pose,
tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities,
std::vector<geometry_msgs::Point> footprint_spec) {
obstacle_costs_.setFootprint(footprint_spec);
//make sure that our configuration doesn't change mid-run
boost::mutex::scoped_lock l(configuration_mutex_);
Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
geometry_msgs::PoseStamped goal_pose = global_plan_.back();
Eigen::Vector3f goal(goal_pose.pose.position.x, goal_pose.pose.position.y, tf::getYaw(goal_pose.pose.orientation));
base_local_planner::LocalPlannerLimits limits = planner_util_->getCurrentLimits();
// prepare cost functions and generators for this run
generator_.initialise(pos,
vel,
goal,
&limits,
vsamples_);
energy_costs_.setLastSpeeds(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));
result_traj_.cost_ = -7;
// find best trajectory by sampling and scoring the samples
std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
if(publish_traj_pc_)
{
base_local_planner::MapGridCostPoint pt;
traj_cloud_->points.clear();
traj_cloud_->width = 0;
traj_cloud_->height = 0;
std_msgs::Header header;
pcl_conversions::fromPCL(traj_cloud_->header, header);
header.stamp = ros::Time::now();
traj_cloud_->header = pcl_conversions::toPCL(header);
for(std::vector<base_local_planner::Trajectory>::iterator t=all_explored.begin(); t != all_explored.end(); ++t)
{
if(t->cost_<0)
continue;
// Fill out the plan
for(unsigned int i = 0; i < t->getPointsSize(); ++i) {
double p_x, p_y, p_th;
t->getPoint(i, p_x, p_y, p_th);
pt.x=p_x;
pt.y=p_y;
pt.z=0;
pt.path_cost=p_th;
pt.total_cost=t->cost_;
traj_cloud_->push_back(pt);
}
}
traj_cloud_pub_.publish(*traj_cloud_);
}
// verbose publishing of point clouds
if (publish_cost_grid_pc_) {
//we'll publish the visualization of the costs to rviz before returning our best trajectory
map_viz_.publishCostCloud(planner_util_->getCostmap());
}
// debrief stateful scoring functions
oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel);
//if we don't have a legal trajectory, we'll just command zero
if (result_traj_.cost_ < 0) {
drive_velocities.setIdentity();
} else {
tf::Vector3 start(result_traj_.xv_, result_traj_.yv_, 0);
drive_velocities.setOrigin(start);
tf::Matrix3x3 matrix;
matrix.setRotation(tf::createQuaternionFromYaw(result_traj_.thetav_));
drive_velocities.setBasis(matrix);
}
return result_traj_;
}
bool goalPositionReached(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y, double xy_goal_tolerance) {
double dist = distance(global_pose.getOrigin().x(), global_pose.getOrigin().y(), goal_x, goal_y);
return fabs(dist) <= xy_goal_tolerance;
}
bool goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th, double yaw_goal_tolerance) {
double yaw = tf::getYaw(global_pose.getRotation());
return fabs(angles::shortest_angular_distance(yaw, goal_th)) <= yaw_goal_tolerance;
}
