Merge pull request #20 from ori-drs/sn-go-back-service

Go back to start service

aicp_core/include/aicp_registration/app.hpp

@@ -44,6 +44,8 @@ struct CommandLineConfig
 namespace aicp {
 
 class App{
+public:
+    typedef std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> PathPoses;
 public:
     App(const CommandLineConfig& cl_cfg,
         RegistrationParams reg_params,
@@ -151,7 +153,7 @@ protected:
         total_correction_ = Eigen::Isometry3d::Identity();
     }
 
-    const CommandLineConfig cl_cfg_;
+    CommandLineConfig cl_cfg_;
 
     std::unique_ptr<AbstractRegistrator> registr_;
     std::unique_ptr<AbstractOverlapper> overlapper_;
@@ -211,7 +213,7 @@ protected:
     bool force_reference_update_;
     Eigen::Isometry3d corrected_pose_;
     Eigen::Isometry3d total_correction_;
-    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poseNodes_;
+    PathPoses poseNodes_;
     // Reference cloud update counters
     int updates_counter_;
     // Current reference pre-filtered

aicp_core/include/aicp_utils/visualizer.hpp

@@ -12,10 +12,10 @@ namespace aicp {
 class Visualizer
 {
 public:
-    Visualizer(){
-
-    }
-//    ~Visualizer();
+    typedef std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> PathPoses;
+public:
+    Visualizer(){}
+    virtual ~Visualizer(){}
 
     virtual void publishCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
                               int param,
@@ -34,10 +34,13 @@ public:
     virtual void publishOctree(octomap::ColorOcTree*& octree,
                                std::string channel_name) = 0;
 
-    virtual void publishPose(Eigen::Isometry3d pose_,
-                             int param,
-                             std::string name,
-                             int64_t utime = -1) = 0;
+    virtual void publishPoses(Eigen::Isometry3d pose_,
+                              int param,
+                              std::string name,
+                              int64_t utime = -1) = 0;
+
+    // Gets
+    virtual const PathPoses& getPath() = 0;
 
 protected:
     // Set global reference frame to zero origin

aicp_core/src/registration/app.cpp

@@ -280,8 +280,10 @@ void App::operator()() {
                 // VISUALIZE first reference cloud
                 reference_vis_ = aligned_clouds_graph_->getCurrentReference()->getCloud();
                 vis_->publishCloud(reference_vis_, 0, "", cloud->getUtime());
-                vis_->publishPose(aligned_clouds_graph_->getCurrentReference()->getCorrectedPose(), 0, "",
-                                  cloud->getUtime());
+
+                // Path
+                vis_->publishPoses(aligned_clouds_graph_->getCurrentReference()->getCorrectedPose(), 0, "",
+                                   cloud->getUtime());
 
                 // Store built map
                 aligned_map_ = aligned_map_ + *reference_vis_;
@@ -394,11 +396,22 @@ void App::operator()() {
                 total_correction_ = fromMatrix4fToIsometry3d(initialT_);
                 updated_correction_ = true;
 
+                // 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())
                 {
-                    vis_->publishPose(aligned_clouds_graph_->getCurrentReference()->getCorrectedPose(), 0, "",
-                                      cloud->getUtime());
+                    // vis_->publishPoses(aligned_clouds_graph_->getCurrentReference()->getCorrectedPose(), 0, "",
+                    //                    cloud->getUtime());
                     reference_vis_ = aligned_clouds_graph_->getCurrentReference()->getCloud();
                     vis_->publishCloud(reference_vis_, 0, "", cloud->getUtime());
                     // Output map
@@ -409,8 +422,8 @@ void App::operator()() {
                 else if(cl_cfg_.localize_against_prior_map &&
                         (aligned_clouds_graph_->getNbClouds()-1) % cl_cfg_.reference_update_frequency == 0)
                 {
-                    vis_->publishPose(aligned_clouds_graph_->getLastCloud()->getCorrectedPose(),
-                                      0, "", cloud->getUtime());
+                    vis_->publishPoses(aligned_clouds_graph_->getLastCloud()->getCorrectedPose(),
+                                       0, "", cloud->getUtime());
                     reference_vis_ = aligned_clouds_graph_->getLastCloud()->getCloud();
                     vis_->publishCloud(reference_vis_, 0, "", cloud->getUtime());
                     // Add last aligned reference to map

aicp_lcm/include/aicp_lcm/visualizer_lcm.hpp

@@ -9,7 +9,7 @@ class LCMVisualizer : public Visualizer
 {
 public:
     LCMVisualizer(boost::shared_ptr<lcm::LCM>& lcm);
-//    ~LCMVisualizer();
+    ~LCMVisualizer(){}
 
     // Publish cloud lcm
     void publishCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
@@ -32,10 +32,14 @@ public:
                        string channel_name);
 
     // Publish corrected pose lcm
-    void publishPose(Eigen::Isometry3d pose_,
-                     int param,
-                     string name,
-                     int64_t utime);
+    void publishPoses(Eigen::Isometry3d pose_,
+                      int param,
+                      string name,
+                      int64_t utime);
+
+    // Gets
+    const PathPoses& getPath()
+    {}
 
 private:
     boost::shared_ptr<lcm::LCM> lcm_;

aicp_lcm/src/visualizer_lcm.cpp

@@ -31,10 +31,10 @@ void LCMVisualizer::publishOctree(octomap::ColorOcTree*& octree,
     publishOctreeToLCM(lcm_, octree, channel_name);
 }
 
-void LCMVisualizer::publishPose(Isometry3d pose_,
-                                int param,
-                                string name,
-                                int64_t utime = -1)
+void LCMVisualizer::publishPoses(Isometry3d pose_,
+                                 int param,
+                                 string name,
+                                 int64_t utime = -1)
 {
     // TO DO: fix this part
 }

aicp_ros/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS aicp_core
                                         eigen_conversions
                                         pcl_conversions
                                         tf_conversions
+                                        std_srvs
                                         std_msgs
                                         sensor_msgs
                                         geometry_msgs
@@ -27,6 +28,7 @@ catkin_package(
                  eigen_conversions
                  pcl_conversions
                  tf_conversions
+                 std_srvs
                  std_msgs
                  sensor_msgs
                  geometry_msgs
@@ -41,6 +43,7 @@ include_directories(
 
 add_library(${PROJECT_NAME} SHARED src/app_ros.cpp
                                    src/visualizer_ros.cpp
+                                   src/talker_ros.cpp
                                    src/velodyne_accumulator.cpp)
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})

aicp_ros/include/aicp_ros/app_ros.hpp

@@ -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 {
@@ -24,6 +26,7 @@ public:
     inline ~AppROS() {
         delete accu_;
         delete vis_ros_;
+        delete talk_ros_;
     }
 
     // Subscriber callabacks
@@ -34,6 +37,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 +59,8 @@ private:
 
     // ROS only visualizer
     ROSVisualizer* vis_ros_;
+    // ROS talker
+    ROSTalker* talk_ros_;
 
     // Tool functions
     void getPoseAsIsometry3d(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose_msg,

aicp_ros/include/aicp_ros/talker_ros.hpp

+#pragma once
+
+#include "ros/node_handle.h"
+
+#include <Eigen/Dense>
+#include <Eigen/StdVector>
+
+#include <geometry_msgs/PoseArray.h>
+
+namespace aicp {
+
+class ROSTalker
+{
+public:
+    typedef std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> PathPoses;
+public:
+    ROSTalker(ros::NodeHandle& nh, std::string fixed_frame);
+
+    // Publish footstep plan
+    void publishFootstepPlan(PathPoses& path,
+                             int64_t utime,
+                             bool reverse_path = false);
+    void reversePath(PathPoses& path);
+
+private:
+    ros::NodeHandle& nh_;
+    ros::Publisher footstep_plan_pub_;
+    std::string fixed_frame_; // map or map_test
+};
+}

aicp_ros/include/aicp_ros/visualizer_ros.hpp

@@ -22,7 +22,7 @@ class ROSVisualizer : public Visualizer
 public:
 
     ROSVisualizer(ros::NodeHandle& nh, std::string fixed_frame);
-//    ~ROSVisualizer();
+    ~ROSVisualizer(){}
 
     // Publish cloud
     void publishCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
@@ -44,13 +44,20 @@ public:
     void publishOctree(octomap::ColorOcTree*& octree,
                        std::string channel_name);
 
-    // Publish corrected pose
-    void publishPose(Eigen::Isometry3d pose,
-                     int param, std::string name, int64_t utime);
+    // 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);
 
+    // Gets
+    const PathPoses& getPath(){
+        return path_;
+    }
+
 private:
     ros::NodeHandle& nh_;
     ros::Publisher cloud_pub_;
@@ -60,7 +67,7 @@ private:
     // Duplicates the list in collections renderer. assumed to be 3xN colors
     std::vector<double> colors_;
     // Path (vector of poses)
-    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> path_;
+    PathPoses path_;
 
     std::string fixed_frame_; // map or map_test
     std::string odom_frame_;

aicp_ros/launch/aicp.launch

@@ -9,6 +9,7 @@
   <arg name="load_map_from_file"               default="false"/>
   <arg name="localize_against_prior_map"       default="false"/>
   <arg name="localize_against_built_map"       default="false"/>
+  <arg name="merge_aligned_clouds_to_map"      default="true"/>
   
   <!-- Registration params -->
   <arg name="registration_default_config_file" default="$(find aicp_core)/config/icp/icp_autotuned_default.yaml" />
@@ -45,7 +46,7 @@
     <!-- Prior map params -->
     <param name="crop_map_around_base"          value="15.0" /> <!-- box dimesions: value*2 x value*2 -->
                                                                 <!-- 15.0 to generate Ground Truth (David IROS19) -->
-    <param name="merge_aligned_clouds_to_map"   value="true" /> <!-- true to generate Ground Truth (David IROS19) -->
+    <param name="merge_aligned_clouds_to_map"   value="$(arg merge_aligned_clouds_to_map)" /> <!-- true to generate Ground Truth (David IROS19) -->
     <!-- Reference update policy -->
     <param name="failure_prediction_mode"       value="false" />
     <param name="reference_update_frequency"    value="5" />

aicp_ros/launch/aicp_localization_only.launch

@@ -7,12 +7,14 @@
   <arg name="map_from_file_path"           default="$(find gazebo_worlds_drs)/point_clouds/fire_college.ply"/>
   <arg name="load_map_from_file"           default="true"/>
   <arg name="localize_against_prior_map"   default="true"/>
+  <arg name="merge_aligned_clouds_to_map"  default="true"/>
 
   <include file="$(find aicp_ros)/launch/aicp.launch">
-    <arg name="fixed_frame"                value="$(arg fixed_frame)"/>
-    <arg name="map_from_file_path"         value="$(arg map_from_file_path)"/>
-    <arg name="load_map_from_file"         value="$(arg load_map_from_file)"/>
-    <arg name="localize_against_prior_map" value="$(arg localize_against_prior_map)"/>
+    <arg name="fixed_frame"                 value="$(arg fixed_frame)"/>
+    <arg name="map_from_file_path"          value="$(arg map_from_file_path)"/>
+    <arg name="load_map_from_file"          value="$(arg load_map_from_file)"/>
+    <arg name="localize_against_prior_map"  value="$(arg localize_against_prior_map)"/>
+    <arg name="merge_aligned_clouds_to_map" value="$(arg merge_aligned_clouds_to_map)"/>
   </include>
   
   <!-- Initial pose guess -->

aicp_ros/launch/view_recorded_rosbag.launch → aicp_ros/launch/view_recorded_rosbag_aicp.launch

@@ -15,7 +15,7 @@
 
   <arg name="data_package"            default="gazebo_worlds"/>
   <arg name="world"                   default="empty"/>
-  <arg name="rviz_config_path"        default="$(find aicp)/config/rviz/config_aicp.rviz"/>
+  <arg name="rviz_config_path"        default="$(find aicp_ros)/config/rviz/config_aicp.rviz"/>
   <arg name="rqt_perspective_path"    default="$(find anymal_b_navigation_ui)/config/rqt/rqt.perspective"/>
 
   <!-- User interface -->

aicp_ros/package.xml

@@ -17,6 +17,7 @@
   <build_depend>eigen_conversions</build_depend>
   <build_depend>pcl_conversions</build_depend>
   <build_depend>tf_conversions</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
@@ -27,6 +28,7 @@
   <run_depend>eigen_conversions</run_depend>
   <run_depend>pcl_conversions</run_depend>
   <run_depend>tf_conversions</run_depend>
+  <run_depend>std_srvs</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>

aicp_ros/src/aicp_ros_node.cpp

@@ -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...");
 

aicp_ros/src/app_ros.cpp

@@ -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();
@@ -84,7 +86,7 @@ void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedCon
                                                           // body -> reference * world -> body
     // Publish initial guess interactive marker
     if (!pose_initialized_)
-        vis_->publishPose(corrected_pose_, 0, "", ros::Time::now().toNSec() / 1000);
+        vis_->publishPoses(corrected_pose_, 0, "", ros::Time::now().toNSec() / 1000);
 
     // Publish fixed_frame to odom tf
     ros::Time msg_time(pose_msg_in->header.stamp.sec, pose_msg_in->header.stamp.nsec);
@@ -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,41 @@ 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());
+    }
+
+    // Change to localization only settings
+    cl_cfg_.localize_against_prior_map = true;
+    pose_marker_initialized_ = true;
+
+    // Set path back
+    PathPoses path_forward = vis_->getPath();
+
+    ROS_INFO_STREAM("------------------------------- GO BACK -------------------------------");
+    ROS_INFO_STREAM("[Aicp] Follow path of "
+                    << path_forward.size()
+                    << " poses back to origin.");
+    ROS_INFO_STREAM("-----------------------------------------------------------------------");
+
+    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);
+
+    return true;
+}
+
 void AppROS::getPoseAsIsometry3d(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose_msg,
                                  Eigen::Isometry3d& eigen_pose)
 {

aicp_ros/src/talker_ros.cpp

+#include "aicp_ros/talker_ros.hpp"
+
+//#include <eigen_conversions/eigen_msg.h>
+
+namespace aicp {
+
+ROSTalker::ROSTalker(ros::NodeHandle& nh, std::string fixed_frame) : nh_(nh),
+                                                                     fixed_frame_(fixed_frame)
+{
+    footstep_plan_pub_ = nh_.advertise<geometry_msgs::PoseArray>("/aicp/footstep_plan_request_list",10);
+}
+
+void ROSTalker::publishFootstepPlan(PathPoses& path,
+                                    int64_t utime,
+                                    bool reverse_path){
+
+    if(reverse_path)
+        reversePath(path);
+
+    geometry_msgs::PoseArray path_msg;
+    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){
+        geometry_msgs::Pose p;
+        p.position.x = path[i].translation().x();
+        p.position.y = path[i].translation().y();
+        p.position.z = path[i].translation().z();
+        Eigen::Matrix3d pose_rot = path[i].rotation();
+        Eigen::Quaterniond pose_rot_q(pose_rot);
+        p.orientation.x = pose_rot_q.x();
+        p.orientation.y = pose_rot_q.y();
+        p.orientation.z = pose_rot_q.z();
+        p.orientation.w = pose_rot_q.w();
+
+        path_msg.poses.push_back(p);
+    }
+
+    footstep_plan_pub_.publish(path_msg);
+}
+
+void ROSTalker::reversePath(PathPoses& 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();
+    }
+}
+}

aicp_ros/src/visualizer_ros.cpp

@@ -48,8 +48,8 @@ ROSVisualizer::ROSVisualizer(ros::NodeHandle& nh, string fixed_frame) : nh_(nh),
          0.5, 1.0, 0.5,
          0.5, 0.5, 1.0};
 
-    odom_frame_ = "/odom";
-    base_frame_ = "/base";
+    odom_frame_ = "odom";
+    base_frame_ = "base";
 }
 
 void ROSVisualizer::publishCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
@@ -127,20 +127,25 @@ void ROSVisualizer::publishMap(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
         ROS_WARN_STREAM("[ROSVisualizer] Unknown channel. Map not published.");
 }
 
-void ROSVisualizer::publishPose(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);
     }