43 lines
1.4 KiB
C++
43 lines
1.4 KiB
C++
#ifndef CONVERSION_H_
|
|
#define CONVERSION_H_
|
|
#include "curve_common.h"
|
|
|
|
#include <Eigen/Geometry>
|
|
#include <robot_geometry_msgs/Point.h>
|
|
#include <robot_geometry_msgs/Quaternion.h>
|
|
|
|
inline Eigen::Vector3d EigenVecter3dFromPointMsg(const robot_geometry_msgs::Point& msg) {
|
|
return Eigen::Vector3d(msg.x, msg.y, msg.z);
|
|
}
|
|
|
|
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::PoseStamped> &plan)
|
|
{
|
|
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
|
|
EigenTrajectoryPoint eigen_trajectory_point;
|
|
|
|
for(int i = 0; i < plan.size(); i++)
|
|
{
|
|
eigen_trajectory_point.position(0) = plan.at(i).pose.position.x;
|
|
eigen_trajectory_point.position(1) = plan.at(i).pose.position.y;
|
|
eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
|
|
}
|
|
|
|
return eigen_trajectory_point_vec;
|
|
}
|
|
|
|
inline EigenTrajectoryPoint::Vector EigenTrajectoryVectorFromVector(const std::vector<robot_geometry_msgs::Point> &discreate_point_vec)
|
|
{
|
|
EigenTrajectoryPoint::Vector eigen_trajectory_point_vec;
|
|
EigenTrajectoryPoint eigen_trajectory_point;
|
|
|
|
for(int i = 0; i < discreate_point_vec.size(); i++)
|
|
{
|
|
eigen_trajectory_point.position(0) = discreate_point_vec.at(i).x;
|
|
eigen_trajectory_point.position(1) = discreate_point_vec.at(i).y;
|
|
eigen_trajectory_point_vec.push_back(eigen_trajectory_point);
|
|
}
|
|
|
|
return eigen_trajectory_point_vec;
|
|
}
|
|
|
|
#endif //CONVERSION_H_
|