27 #ifndef XPP_ROS_CONVERSIONS_H_ 28 #define XPP_ROS_CONVERSIONS_H_ 32 #include <xpp_msgs/StateLin3d.h> 33 #include <xpp_msgs/State6d.h> 34 #include <xpp_msgs/RobotStateCartesian.h> 35 #include <xpp_msgs/RobotStateCartesianTrajectory.h> 54 point.
p_.x() = ros.pos.x;
55 point.
p_.y() = ros.pos.y;
56 point.
p_.z() = ros.pos.z;
58 point.
v_.x() = ros.vel.x;
59 point.
v_.y() = ros.vel.y;
60 point.
v_.z() = ros.vel.z;
62 point.
a_.x() = ros.acc.x;
63 point.
a_.y() = ros.acc.y;
64 point.
a_.z() = ros.acc.z;
69 static xpp_msgs::StateLin3d
72 xpp_msgs::StateLin3d
ros;
73 ros.pos.x = xpp.
p_.x();
74 ros.pos.y = xpp.
p_.y();
75 ros.pos.z = xpp.
p_.z();
77 ros.vel.x = xpp.
v_.x();
78 ros.vel.y = xpp.
v_.y();
79 ros.vel.z = xpp.
v_.z();
81 ros.acc.x = xpp.
a_.x();
82 ros.acc.y = xpp.
a_.y();
83 ros.acc.z = xpp.
a_.z();
93 vec << ros.x, ros.y, ros.z;
115 for (
auto ee :
xpp.GetEEsOrdered())
121 static Eigen::Quaterniond
124 Eigen::Quaterniond
xpp;
133 static geometry_msgs::Quaternion
136 geometry_msgs::Quaternion
ros;
145 static xpp_msgs::State6d
148 xpp_msgs::State6d msg;
150 msg.pose.position = ToRos<geometry_msgs::Point>(xpp.
lin.
p_);
151 msg.twist.linear = ToRos<geometry_msgs::Vector3>(xpp.
lin.
v_);
152 msg.accel.linear = ToRos<geometry_msgs::Vector3>(xpp.
lin.
a_);
155 msg.twist.angular = ToRos<geometry_msgs::Vector3>(xpp.
ang.
w);
156 msg.accel.angular = ToRos<geometry_msgs::Vector3>(xpp.
ang.
wd);
177 static xpp_msgs::RobotStateCartesian
180 xpp_msgs::RobotStateCartesian
ros;
188 ros.ee_forces. push_back(ToRos<geometry_msgs::Vector3>(xpp.
ee_forces_.
at(ee)));
198 int n_ee = ros.ee_motion.size();
202 xpp.
t_global_ = ros.time_from_start.toSec();
213 static xpp_msgs::RobotStateCartesianTrajectory
216 xpp_msgs::RobotStateCartesianTrajectory msg;
218 for (
const auto state : xpp) {
219 auto state_msg =
ToRos(state);
220 msg.points.push_back(state_msg);
226 static std::vector<RobotStateCartesian>
227 ToXpp(
const xpp_msgs::RobotStateCartesianTrajectory&
ros)
229 std::vector<RobotStateCartesian> xpp_vec;
231 for (
const auto ros_state : ros.points) {
233 xpp_vec.push_back(
xpp);
static geometry_msgs::Quaternion ToRos(const Eigen::Quaterniond xpp)
static xpp_msgs::StateLin3d ToRos(const StateLin3d &xpp)
StateLin3d & at(EndeffectorID ee)
static T ToRos(const Vector3d &xpp)
static std::vector< RobotStateCartesian > ToXpp(const xpp_msgs::RobotStateCartesianTrajectory &ros)
static xpp_msgs::RobotStateCartesianTrajectory ToRos(const std::vector< RobotStateCartesian > &xpp)
EndeffectorsMotion ee_motion_
static xpp_msgs::RobotStateCartesian ToRos(const RobotStateCartesian &xpp)
static State3d ToXpp(const xpp_msgs::State6d &ros)
Converts between xpp-states types and xpp-messages.
static RobotStateCartesian ToXpp(const xpp_msgs::RobotStateCartesian &ros)
static xpp_msgs::State6d ToRos(const State3d &xpp)
std::vector< EndeffectorID > GetEEsOrdered() const
static Endeffectors< Vector3d > ToXpp(const std::vector< T > &ros)
static Eigen::Quaterniond ToXpp(const geometry_msgs::Quaternion ros)
Endeffectors< Vector3d > ee_forces_
EndeffectorsContact ee_contact_
static Vector3d ToXpp(const T &ros)
static StateLin3d ToXpp(const xpp_msgs::StateLin3d &ros)