Commit e7fb8171 authored by simalpha's avatar simalpha
Browse files

Reverse path.

parent 4bf1a1aa
......@@ -16,7 +16,9 @@ public:
// Publish footstep plan
void publishFootstepPlan(std::vector<Eigen::Isometry3d> &path,
int64_t utime);
int64_t utime,
bool reverse_path = false);
void reversePath(std::vector<Eigen::Isometry3d>& path);
private:
ros::NodeHandle& nh_;
......
......@@ -294,15 +294,15 @@ bool AppROS::goBackRequest()
pose_marker_initialized_ = true;
// Set path back
std::vector<Eigen::Isometry3d> path_reversed = vis_->getPath();
std::reverse(path_reversed.begin(),path_reversed.end());
std::vector<Eigen::Isometry3d> path_forward = vis_->getPath();
ROS_INFO_STREAM("------------------------------- GO BACK -------------------------------");
ROS_INFO_STREAM("[Aicp] Follow path of "
<< path_reversed.size()
<< path_forward.size()
<< " poses back to origin.");
ROS_INFO_STREAM("-----------------------------------------------------------------------");
talk_ros_->publishFootstepPlan(path_reversed, ros::Time::now().toNSec() / 1000);
talk_ros_->publishFootstepPlan(path_forward, ros::Time::now().toNSec() / 1000, true);
pcl::PointCloud<pcl::PointXYZ>::Ptr prior_map_ptr = prior_map_->getCloud();
vis_->publishMap(prior_map_ptr, prior_map_->getUtime(), 0);
......
......@@ -11,7 +11,11 @@ ROSTalker::ROSTalker(ros::NodeHandle& nh, std::string fixed_frame) : nh_(nh),
}
void ROSTalker::publishFootstepPlan(std::vector<Eigen::Isometry3d>& path,
int64_t utime){
int64_t utime,
bool reverse_path){
if(reverse_path)
reversePath(path);
geometry_msgs::PoseArray path_msg;
int secs = utime*1E-6;
......@@ -36,4 +40,19 @@ void ROSTalker::publishFootstepPlan(std::vector<Eigen::Isometry3d>& path,
footstep_plan_pub_.publish(path_msg);
}
void ROSTalker::reversePath(std::vector<Eigen::Isometry3d>& path){
std::reverse(path.begin(),path.end());
for (size_t i = 0; i < path.size(); ++i){
// Turn 90 degrees about z-axis
Eigen::Matrix3d rot;
rot = Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitX())
* Eigen::AngleAxisd(0*M_PI/180, Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(180*M_PI/180, Eigen::Vector3d::UnitZ());
path[i].linear() = rot * path[i].rotation();
}
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment