Commit 5730bebd authored by simalpha's avatar simalpha
Browse files

Typedef for std::vector of Eigen::Isometry3d.

parent 9e42019a
......@@ -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,
......@@ -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
......
......@@ -11,6 +11,8 @@ namespace aicp {
class Visualizer
{
public:
typedef std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> PathPoses;
public:
Visualizer(){}
virtual ~Visualizer(){}
......@@ -38,7 +40,7 @@ public:
int64_t utime = -1) = 0;
// Gets
virtual const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& getPath() = 0;
virtual const PathPoses& getPath() = 0;
protected:
// Set global reference frame to zero origin
......
......@@ -38,7 +38,7 @@ public:
int64_t utime);
// Gets
const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& getPath()
const PathPoses& getPath()
{}
private:
......
......@@ -12,14 +12,15 @@ 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(std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& path,
void publishFootstepPlan(PathPoses& path,
int64_t utime,
bool reverse_path = false);
void reversePath(std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& path);
void reversePath(PathPoses& path);
private:
ros::NodeHandle& nh_;
......
......@@ -52,7 +52,7 @@ public:
void publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time);
// Gets
const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& getPath(){
const PathPoses& getPath(){
return path_;
}
......@@ -65,7 +65,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_;
......
......@@ -294,7 +294,7 @@ bool AppROS::goBackRequest()
pose_marker_initialized_ = true;
// Set path back
std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> path_forward = vis_->getPath();
PathPoses path_forward = vis_->getPath();
ROS_INFO_STREAM("------------------------------- GO BACK -------------------------------");
ROS_INFO_STREAM("[Aicp] Follow path of "
......
......@@ -5,12 +5,12 @@
namespace aicp {
ROSTalker::ROSTalker(ros::NodeHandle& nh, std::string fixed_frame) : nh_(nh),
fixed_frame_(fixed_frame)
fixed_frame_(fixed_frame)
{
footstep_plan_pub_ = nh_.advertise<geometry_msgs::PoseArray>("/aicp/footstep_plan_request_list",10);
}
void ROSTalker::publishFootstepPlan(std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& path,
void ROSTalker::publishFootstepPlan(PathPoses& path,
int64_t utime,
bool reverse_path){
......@@ -41,7 +41,7 @@ void ROSTalker::publishFootstepPlan(std::vector<Eigen::Isometry3d, Eigen::aligne
footstep_plan_pub_.publish(path_msg);
}
void ROSTalker::reversePath(std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& path){
void ROSTalker::reversePath(PathPoses& path){
std::reverse(path.begin(),path.end());
......
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