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);*/ }
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); */ }
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); }
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); }
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; } } return false; }
/* * 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_; }
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); } }
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; }
//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; }
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); }
/* * 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 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; }