Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #ifndef XPP_ROS_CONVERSIONS_H_
00028 #define XPP_ROS_CONVERSIONS_H_
00029
00030 #include <vector>
00031
00032 #include <xpp_msgs/StateLin3d.h>
00033 #include <xpp_msgs/State6d.h>
00034 #include <xpp_msgs/RobotStateCartesian.h>
00035 #include <xpp_msgs/RobotStateCartesianTrajectory.h>
00036
00037 #include <xpp_states/state.h>
00038 #include <xpp_states/robot_state_cartesian.h>
00039
00040 namespace xpp {
00041
00048 struct Convert {
00049
00050 static StateLin3d
00051 ToXpp(const xpp_msgs::StateLin3d& ros)
00052 {
00053 StateLin3d point;
00054 point.p_.x() = ros.pos.x;
00055 point.p_.y() = ros.pos.y;
00056 point.p_.z() = ros.pos.z;
00057
00058 point.v_.x() = ros.vel.x;
00059 point.v_.y() = ros.vel.y;
00060 point.v_.z() = ros.vel.z;
00061
00062 point.a_.x() = ros.acc.x;
00063 point.a_.y() = ros.acc.y;
00064 point.a_.z() = ros.acc.z;
00065
00066 return point;
00067 }
00068
00069 static xpp_msgs::StateLin3d
00070 ToRos(const StateLin3d& xpp)
00071 {
00072 xpp_msgs::StateLin3d ros;
00073 ros.pos.x = xpp.p_.x();
00074 ros.pos.y = xpp.p_.y();
00075 ros.pos.z = xpp.p_.z();
00076
00077 ros.vel.x = xpp.v_.x();
00078 ros.vel.y = xpp.v_.y();
00079 ros.vel.z = xpp.v_.z();
00080
00081 ros.acc.x = xpp.a_.x();
00082 ros.acc.y = xpp.a_.y();
00083 ros.acc.z = xpp.a_.z();
00084
00085 return ros;
00086 }
00087
00088 template<typename T>
00089 static Vector3d
00090 ToXpp(const T& ros)
00091 {
00092 Vector3d vec;
00093 vec << ros.x, ros.y, ros.z;
00094 return vec;
00095 }
00096
00097 template<typename T>
00098 static T
00099 ToRos(const Vector3d& xpp)
00100 {
00101 T ros;
00102 ros.x = xpp.x();
00103 ros.y = xpp.y();
00104 ros.z = xpp.z();
00105
00106 return ros;
00107 }
00108
00109 template<typename T>
00110 static Endeffectors<Vector3d>
00111 ToXpp(const std::vector<T>& ros)
00112 {
00113 Endeffectors<Vector3d> xpp(ros.size());
00114
00115 for (auto ee : xpp.GetEEsOrdered())
00116 xpp.at(ee) = ToXpp(ros.at(ee));
00117
00118 return xpp;
00119 }
00120
00121 static Eigen::Quaterniond
00122 ToXpp(const geometry_msgs::Quaternion ros)
00123 {
00124 Eigen::Quaterniond xpp;
00125 xpp.w() = ros.w;
00126 xpp.x() = ros.x;
00127 xpp.y() = ros.y;
00128 xpp.z() = ros.z;
00129
00130 return xpp;
00131 }
00132
00133 static geometry_msgs::Quaternion
00134 ToRos(const Eigen::Quaterniond xpp)
00135 {
00136 geometry_msgs::Quaternion ros;
00137 ros.w = xpp.w();
00138 ros.x = xpp.x();
00139 ros.y = xpp.y();
00140 ros.z = xpp.z();
00141
00142 return ros;
00143 }
00144
00145 static xpp_msgs::State6d
00146 ToRos(const State3d& xpp)
00147 {
00148 xpp_msgs::State6d msg;
00149
00150 msg.pose.position = ToRos<geometry_msgs::Point>(xpp.lin.p_);
00151 msg.twist.linear = ToRos<geometry_msgs::Vector3>(xpp.lin.v_);
00152 msg.accel.linear = ToRos<geometry_msgs::Vector3>(xpp.lin.a_);
00153
00154 msg.pose.orientation = ToRos(xpp.ang.q);
00155 msg.twist.angular = ToRos<geometry_msgs::Vector3>(xpp.ang.w);
00156 msg.accel.angular = ToRos<geometry_msgs::Vector3>(xpp.ang.wd);
00157
00158 return msg;
00159 }
00160
00161 static State3d
00162 ToXpp(const xpp_msgs::State6d& ros)
00163 {
00164 State3d xpp;
00165
00166 xpp.lin.p_ = ToXpp(ros.pose.position);
00167 xpp.lin.v_ = ToXpp(ros.twist.linear);
00168 xpp.lin.a_ = ToXpp(ros.accel.linear);
00169
00170 xpp.ang.q = ToXpp(ros.pose.orientation);
00171 xpp.ang.w = ToXpp(ros.twist.angular);
00172 xpp.ang.wd = ToXpp(ros.accel.angular);
00173
00174 return xpp;
00175 }
00176
00177 static xpp_msgs::RobotStateCartesian
00178 ToRos(const RobotStateCartesian& xpp)
00179 {
00180 xpp_msgs::RobotStateCartesian ros;
00181
00182 ros.base = ToRos(xpp.base_);
00183 ros.time_from_start = ros::Duration(xpp.t_global_);
00184
00185 for (auto ee : xpp.ee_contact_.GetEEsOrdered()) {
00186 ros.ee_motion. push_back(ToRos(xpp.ee_motion_.at(ee)));
00187 ros.ee_contact.push_back(xpp.ee_contact_.at(ee));
00188 ros.ee_forces. push_back(ToRos<geometry_msgs::Vector3>(xpp.ee_forces_.at(ee)));
00189 }
00190
00191 return ros;
00192 }
00193
00194 static RobotStateCartesian
00195 ToXpp(const xpp_msgs::RobotStateCartesian& ros)
00196 {
00197
00198 int n_ee = ros.ee_motion.size();
00199 RobotStateCartesian xpp(n_ee);
00200
00201 xpp.base_ = ToXpp(ros.base);
00202 xpp.t_global_ = ros.time_from_start.toSec();
00203
00204 for (auto ee : xpp.ee_contact_.GetEEsOrdered()) {
00205 xpp.ee_motion_.at(ee) = ToXpp(ros.ee_motion.at(ee));
00206 xpp.ee_contact_.at(ee) = ros.ee_contact.at(ee);
00207 xpp.ee_forces_.at(ee) = ToXpp(ros.ee_forces.at(ee));
00208 }
00209
00210 return xpp;
00211 }
00212
00213 static xpp_msgs::RobotStateCartesianTrajectory
00214 ToRos(const std::vector<RobotStateCartesian>& xpp)
00215 {
00216 xpp_msgs::RobotStateCartesianTrajectory msg;
00217
00218 for (const auto state : xpp) {
00219 auto state_msg = ToRos(state);
00220 msg.points.push_back(state_msg);
00221 }
00222
00223 return msg;
00224 }
00225
00226 static std::vector<RobotStateCartesian>
00227 ToXpp(const xpp_msgs::RobotStateCartesianTrajectory& ros)
00228 {
00229 std::vector<RobotStateCartesian> xpp_vec;
00230
00231 for (const auto ros_state : ros.points) {
00232 auto xpp = ToXpp(ros_state);
00233 xpp_vec.push_back(xpp);
00234 }
00235
00236 return xpp_vec;
00237 }
00238
00239 };
00240
00241 }
00242
00243 #endif