Commit 9802de1e authored by simalpha's avatar simalpha
Browse files

Towards go back functionality: footstep plan service...

parent 509e8875
......@@ -151,7 +151,7 @@ protected:
total_correction_ = Eigen::Isometry3d::Identity();
}
const CommandLineConfig cl_cfg_;
CommandLineConfig cl_cfg_;
std::unique_ptr<AbstractRegistrator> registr_;
std::unique_ptr<AbstractOverlapper> overlapper_;
......
......@@ -6,10 +6,12 @@
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <aicp_srv/ProcessFile.h>
#include <std_srvs/Trigger.h>
#include "aicp_registration/app.hpp"
#include "velodyne_accumulator.hpp"
#include "visualizer_ros.hpp"
#include "talker_ros.hpp"
namespace aicp {
class AppROS : public App {
......@@ -34,6 +36,8 @@ public:
// Advertise services
bool loadMapFromFileCallBack(aicp_srv::ProcessFile::Request& request, aicp_srv::ProcessFile::Response& response);
bool loadMapFromFile(const std::string& file_path);
bool goBackRequestCallBack(std_srvs::Trigger::Request& request, std_srvs::Trigger::Response& response);
bool goBackRequest();
void run();
......@@ -54,6 +58,8 @@ private:
// ROS only visualizer
ROSVisualizer* vis_ros_;
// ROS talker
ROSTalker* talk_ros_;
// Tool functions
void getPoseAsIsometry3d(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose_msg,
......
......@@ -107,6 +107,7 @@ int main(int argc, char** argv){
// Advertise services (using service published by anybotics icp_tools ui)
ros::ServiceServer load_map_server_ = nh.advertiseService("/icp_tools/load_map_from_file", &aicp::AppROS::loadMapFromFileCallBack, app.get());
ros::ServiceServer go_back_server_ = nh.advertiseService("/aicp/go_back_request", &aicp::AppROS::goBackRequestCallBack, app.get());
ROS_INFO_STREAM("[Aicp] Waiting for input messages...");
......
......@@ -29,6 +29,8 @@ AppROS::AppROS(ros::NodeHandle &nh,
// Visualizer
vis_ = new ROSVisualizer(nh_, cl_cfg_.fixed_frame);
vis_ros_ = new ROSVisualizer(nh_, cl_cfg_.fixed_frame);
// Talker
talk_ros_ = new ROSTalker(nh_, cl_cfg_.fixed_frame);
// Init pose to identity
world_to_body_ = Eigen::Isometry3d::Identity();
......@@ -230,8 +232,7 @@ void AppROS::interactionMarkerCallBack(const geometry_msgs::PoseStampedConstPtr&
bool AppROS::loadMapFromFileCallBack(aicp_srv::ProcessFile::Request& request, aicp_srv::ProcessFile::Response& response)
{
response.success = loadMapFromFile(request.file_path);
return true;
return response.success = loadMapFromFile(request.file_path);
}
bool AppROS::loadMapFromFile(const std::string& file_path)
......@@ -273,6 +274,40 @@ bool AppROS::loadMapFromFile(const std::string& file_path)
return true;
}
bool AppROS::goBackRequestCallBack(std_srvs::Trigger::Request& request, std_srvs::Trigger::Response& response)
{
return response.success = goBackRequest();
}
bool AppROS::goBackRequest()
{
if (!cl_cfg_.localize_against_prior_map){
// Set map to localize against
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_map_ptr = aligned_map_.makeShared();
prior_map_ = new AlignedCloud(ros::Time::now().toNSec() / 1000,
aligned_map_ptr,
Eigen::Isometry3d::Identity());
}
ROS_INFO_STREAM("------------------------------- GO BACK -------------------------------");
ROS_INFO_STREAM("[Aicp] Go back requested! Turn and follow previous path back to origin.");
ROS_INFO_STREAM("-----------------------------------------------------------------------");
// Change to localization only settings
cl_cfg_.localize_against_prior_map = true;
pose_marker_initialized_ = true;
// Set path back
std::vector<Eigen::Isometry3d> path_reversed = vis_ros_->getPath();
std::reverse(path_reversed.begin(),path_reversed.end());
cout << "path_reversed.size(): " << path_reversed.size() << endl;
talk_ros_->publishFootstepPlan(path_reversed, ros::Time::now().toNSec() / 1000);
pcl::PointCloud<pcl::PointXYZ>::Ptr prior_map_ptr = prior_map_->getCloud();
vis_->publishMap(prior_map_ptr, prior_map_->getUtime(), 0);
return true;
}
void AppROS::getPoseAsIsometry3d(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose_msg,
Eigen::Isometry3d& eigen_pose)
{
......
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