Commit 0f0013e3 authored by simalpha's avatar simalpha
Browse files

Add pose to path every >1 meter.

parent c42b03de
......@@ -396,9 +396,16 @@ void App::operator()() {
total_correction_ = fromMatrix4fToIsometry3d(initialT_);
updated_correction_ = true;
// Path
vis_->publishPoses(aligned_clouds_graph_->getLastCloud()->getCorrectedPose(), 0, "",
cloud->getUtime());
// Path (save and visualize)
// Ensure robot moves between stored poses
Eigen::Isometry3d relative_motion = vis_->getPath().back().inverse() *
aligned_clouds_graph_->getLastCloud()->getCorrectedPose();
double dist = relative_motion.translation().norm();
if (dist > 1.0)
{
vis_->publishPoses(aligned_clouds_graph_->getLastCloud()->getCorrectedPose(), 0, "",
cloud->getUtime());
}
// Store aligned map and VISUALIZE
if(aligned_clouds_graph_->getLastCloud()->isReference())
......
......@@ -44,9 +44,11 @@ public:
void publishOctree(octomap::ColorOcTree*& octree,
std::string channel_name);
// Publish corrected pose
// Publish corrected poses
void publishPoses(Eigen::Isometry3d pose,
int param, std::string name, int64_t utime);
void publishPoses(PathPoses poses,
int param, std::string name, int64_t utime);
// Publish tf from fixed_frame to odom
void publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time);
......
......@@ -127,20 +127,25 @@ void ROSVisualizer::publishMap(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
ROS_WARN_STREAM("[ROSVisualizer] Unknown channel. Map not published.");
}
void ROSVisualizer::publishPoses(Eigen::Isometry3d pose, int param, string name, int64_t utime){
void ROSVisualizer::publishPoses(Eigen::Isometry3d pose, int param, std::string name, int64_t utime)
{
path_.push_back(pose);
publishPoses(path_, param, name, utime);
}
void ROSVisualizer::publishPoses(PathPoses poses, int param, string name, int64_t utime){
nav_msgs::Path path_msg;
path_.push_back(pose);
int secs = utime*1E-6;
int nsecs = (utime - (secs*1E6))*1E3;
path_msg.header.stamp = ros::Time(secs, nsecs);
path_msg.header.frame_id = fixed_frame_;
for (size_t i = 0; i < path_.size(); ++i){
for (size_t i = 0; i < poses.size(); ++i){
geometry_msgs::PoseStamped m;
m.header.stamp = ros::Time(secs, nsecs);
m.header.frame_id = fixed_frame_;
tf::poseEigenToMsg(path_[i], m.pose);
tf::poseEigenToMsg(poses[i], m.pose);
path_msg.poses.push_back(m);
}
......
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