Commit 60fe8e82 authored by simalpha's avatar simalpha
Browse files

Fixed vector of Eigen::Isometry3d issue.

parent f6ecee7a
...@@ -38,7 +38,7 @@ public: ...@@ -38,7 +38,7 @@ public:
int64_t utime = -1) = 0; int64_t utime = -1) = 0;
// Gets // Gets
virtual const std::vector<Eigen::Isometry3d>& getPath() = 0; virtual const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& getPath() = 0;
protected: protected:
// Set global reference frame to zero origin // Set global reference frame to zero origin
......
...@@ -38,7 +38,7 @@ public: ...@@ -38,7 +38,7 @@ public:
int64_t utime); int64_t utime);
// Gets // Gets
const std::vector<Eigen::Isometry3d>& getPath() const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& getPath()
{} {}
private: private:
......
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#include "ros/node_handle.h" #include "ros/node_handle.h"
#include <Eigen/Dense> #include <Eigen/Dense>
#include <Eigen/StdVector>
#include <geometry_msgs/PoseArray.h> #include <geometry_msgs/PoseArray.h>
...@@ -15,10 +16,10 @@ public: ...@@ -15,10 +16,10 @@ public:
ROSTalker(ros::NodeHandle& nh, std::string fixed_frame); ROSTalker(ros::NodeHandle& nh, std::string fixed_frame);
// Publish footstep plan // Publish footstep plan
void publishFootstepPlan(std::vector<Eigen::Isometry3d> &path, void publishFootstepPlan(std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& path,
int64_t utime, int64_t utime,
bool reverse_path = false); bool reverse_path = false);
void reversePath(std::vector<Eigen::Isometry3d>& path); void reversePath(std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& path);
private: private:
ros::NodeHandle& nh_; ros::NodeHandle& nh_;
......
...@@ -52,7 +52,7 @@ public: ...@@ -52,7 +52,7 @@ public:
void publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time); void publishFixedFrameToOdomTF(Eigen::Isometry3d& fixed_frame_to_base_eigen, ros::Time msg_time);
// Gets // Gets
const std::vector<Eigen::Isometry3d>& getPath(){ const std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& getPath(){
return path_; return path_;
} }
......
...@@ -294,7 +294,7 @@ bool AppROS::goBackRequest() ...@@ -294,7 +294,7 @@ bool AppROS::goBackRequest()
pose_marker_initialized_ = true; pose_marker_initialized_ = true;
// Set path back // Set path back
std::vector<Eigen::Isometry3d> path_forward = vis_->getPath(); std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> path_forward = vis_->getPath();
ROS_INFO_STREAM("------------------------------- GO BACK -------------------------------"); ROS_INFO_STREAM("------------------------------- GO BACK -------------------------------");
ROS_INFO_STREAM("[Aicp] Follow path of " ROS_INFO_STREAM("[Aicp] Follow path of "
......
...@@ -10,7 +10,7 @@ ROSTalker::ROSTalker(ros::NodeHandle& nh, std::string fixed_frame) : nh_(nh), ...@@ -10,7 +10,7 @@ ROSTalker::ROSTalker(ros::NodeHandle& nh, std::string fixed_frame) : nh_(nh),
footstep_plan_pub_ = nh_.advertise<geometry_msgs::PoseArray>("/position_controller/footstep_plan_request_list",10); footstep_plan_pub_ = nh_.advertise<geometry_msgs::PoseArray>("/position_controller/footstep_plan_request_list",10);
} }
void ROSTalker::publishFootstepPlan(std::vector<Eigen::Isometry3d>& path, void ROSTalker::publishFootstepPlan(std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& path,
int64_t utime, int64_t utime,
bool reverse_path){ bool reverse_path){
...@@ -41,7 +41,7 @@ void ROSTalker::publishFootstepPlan(std::vector<Eigen::Isometry3d>& path, ...@@ -41,7 +41,7 @@ void ROSTalker::publishFootstepPlan(std::vector<Eigen::Isometry3d>& path,
footstep_plan_pub_.publish(path_msg); footstep_plan_pub_.publish(path_msg);
} }
void ROSTalker::reversePath(std::vector<Eigen::Isometry3d>& path){ void ROSTalker::reversePath(std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>>& path){
std::reverse(path.begin(),path.end()); std::reverse(path.begin(),path.end());
......
Supports Markdown
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