Unverified Commit ec0b093e authored by Marco Camurri's avatar Marco Camurri Committed by GitHub
Browse files

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

Go back to start service
parents e356ced4 0f0013e3
......@@ -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
......
......@@ -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
......
......@@ -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
......
......@@ -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_;
......
......@@ -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
}
......
......@@ -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})
......
......@@ -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,
......
#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
};
}
......@@ -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_;
......
......@@ -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" />
......
......@@ -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 -->
......
......@@ -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 -->
......
......@@ -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>
......
......@@ -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();
......@@ -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)
{
......
#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();
}
}
}
......@@ -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);
}
......
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