00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #ifndef MAV_MSGS_CONVERSIONS_H
00024 #define MAV_MSGS_CONVERSIONS_H
00025
00026 #include <geometry_msgs/Point.h>
00027 #include <geometry_msgs/PoseStamped.h>
00028 #include <geometry_msgs/Quaternion.h>
00029 #include <geometry_msgs/TransformStamped.h>
00030 #include <geometry_msgs/Vector3.h>
00031 #include <nav_msgs/Odometry.h>
00032 #include <ros/ros.h>
00033 #include <trajectory_msgs/MultiDOFJointTrajectory.h>
00034
00035 #include "mav_msgs/Actuators.h"
00036 #include "mav_msgs/AttitudeThrust.h"
00037 #include "mav_msgs/RateThrust.h"
00038 #include "mav_msgs/RollPitchYawrateThrust.h"
00039 #include "mav_msgs/TorqueThrust.h"
00040 #include "mav_msgs/common.h"
00041 #include "mav_msgs/default_values.h"
00042 #include "mav_msgs/eigen_mav_msgs.h"
00043
00044 namespace mav_msgs {
00045
00046 inline void eigenAttitudeThrustFromMsg(const AttitudeThrust& msg,
00047 EigenAttitudeThrust* attitude_thrust) {
00048 assert(attitude_thrust != NULL);
00049
00050 attitude_thrust->attitude = quaternionFromMsg(msg.attitude);
00051 attitude_thrust->thrust = vector3FromMsg(msg.thrust);
00052 }
00053
00054 inline void eigenActuatorsFromMsg(const Actuators& msg,
00055 EigenActuators* actuators) {
00056 assert(actuators != NULL);
00057
00058
00059 actuators->angles.resize(msg.angles.size());
00060 for (unsigned int i = 0; i < msg.angles.size(); ++i) {
00061 actuators->angles[i] = msg.angles[i];
00062 }
00063
00064
00065 actuators->angular_velocities.resize(msg.angular_velocities.size());
00066 for (unsigned int i = 0; i < msg.angular_velocities.size(); ++i) {
00067 actuators->angular_velocities[i] = msg.angular_velocities[i];
00068 }
00069
00070
00071
00072 actuators->normalized.resize(msg.normalized.size());
00073 for (unsigned int i = 0; i < msg.normalized.size(); ++i) {
00074 actuators->normalized[i] = msg.normalized[i];
00075 }
00076 }
00077
00078 inline void eigenRateThrustFromMsg(const RateThrust& msg,
00079 EigenRateThrust* rate_thrust) {
00080 assert(rate_thrust != NULL);
00081
00082 rate_thrust->angular_rates = vector3FromMsg(msg.angular_rates);
00083 rate_thrust->thrust = vector3FromMsg(msg.thrust);
00084 }
00085
00086 inline void eigenTorqueThrustFromMsg(const TorqueThrust& msg,
00087 EigenTorqueThrust* torque_thrust) {
00088 assert(torque_thrust != NULL);
00089
00090 torque_thrust->torque = vector3FromMsg(msg.torque);
00091 torque_thrust->thrust = vector3FromMsg(msg.thrust);
00092 }
00093
00094 inline void eigenRollPitchYawrateThrustFromMsg(
00095 const RollPitchYawrateThrust& msg,
00096 EigenRollPitchYawrateThrust* roll_pitch_yawrate_thrust) {
00097 assert(roll_pitch_yawrate_thrust != NULL);
00098
00099 roll_pitch_yawrate_thrust->roll = msg.roll;
00100 roll_pitch_yawrate_thrust->pitch = msg.pitch;
00101 roll_pitch_yawrate_thrust->yaw_rate = msg.yaw_rate;
00102 roll_pitch_yawrate_thrust->thrust = vector3FromMsg(msg.thrust);
00103 }
00104
00105 inline void eigenOdometryFromMsg(const nav_msgs::Odometry& msg,
00106 EigenOdometry* odometry) {
00107 assert(odometry != NULL);
00108 odometry->timestamp_ns = msg.header.stamp.toNSec();
00109 odometry->position_W = mav_msgs::vector3FromPointMsg(msg.pose.pose.position);
00110 odometry->orientation_W_B =
00111 mav_msgs::quaternionFromMsg(msg.pose.pose.orientation);
00112 odometry->velocity_B = mav_msgs::vector3FromMsg(msg.twist.twist.linear);
00113 odometry->angular_velocity_B =
00114 mav_msgs::vector3FromMsg(msg.twist.twist.angular);
00115 odometry->pose_covariance_ =
00116 Eigen::Map<const Eigen::Matrix<double, 6, 6>>(msg.pose.covariance.data());
00117 odometry->twist_covariance_ = Eigen::Map<const Eigen::Matrix<double, 6, 6>>(
00118 msg.twist.covariance.data());
00119 }
00120
00121 inline void eigenTrajectoryPointFromTransformMsg(
00122 const geometry_msgs::TransformStamped& msg,
00123 EigenTrajectoryPoint* trajectory_point) {
00124 assert(trajectory_point != NULL);
00125
00126 ros::Time timestamp = msg.header.stamp;
00127
00128 trajectory_point->timestamp_ns = timestamp.toNSec();
00129 trajectory_point->position_W = vector3FromMsg(msg.transform.translation);
00130 trajectory_point->orientation_W_B = quaternionFromMsg(msg.transform.rotation);
00131 trajectory_point->velocity_W.setZero();
00132 trajectory_point->angular_velocity_W.setZero();
00133 trajectory_point->acceleration_W.setZero();
00134 trajectory_point->angular_acceleration_W.setZero();
00135 trajectory_point->jerk_W.setZero();
00136 trajectory_point->snap_W.setZero();
00137 }
00138
00139
00140
00141
00142 inline void eigenTrajectoryPointFromPoseMsg(
00143 const geometry_msgs::Pose& msg, EigenTrajectoryPoint* trajectory_point) {
00144 assert(trajectory_point != NULL);
00145
00146 trajectory_point->position_W = vector3FromPointMsg(msg.position);
00147 trajectory_point->orientation_W_B = quaternionFromMsg(msg.orientation);
00148 trajectory_point->velocity_W.setZero();
00149 trajectory_point->angular_velocity_W.setZero();
00150 trajectory_point->acceleration_W.setZero();
00151 trajectory_point->angular_acceleration_W.setZero();
00152 trajectory_point->jerk_W.setZero();
00153 trajectory_point->snap_W.setZero();
00154 }
00155
00156 inline void eigenTrajectoryPointFromPoseMsg(
00157 const geometry_msgs::PoseStamped& msg,
00158 EigenTrajectoryPoint* trajectory_point) {
00159 assert(trajectory_point != NULL);
00160
00161 ros::Time timestamp = msg.header.stamp;
00162
00163 trajectory_point->timestamp_ns = timestamp.toNSec();
00164 eigenTrajectoryPointFromPoseMsg(msg.pose, trajectory_point);
00165 }
00166
00189 void EigenMavStateFromEigenTrajectoryPoint(
00190 const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk,
00191 const Eigen::Vector3d& snap, double yaw, double yaw_rate,
00192 double yaw_acceleration, double magnitude_of_gravity,
00193 Eigen::Quaterniond* orientation, Eigen::Vector3d* acceleration_body,
00194 Eigen::Vector3d* angular_velocity_body,
00195 Eigen::Vector3d* angular_acceleration_body);
00196
00199 inline void EigenMavStateFromEigenTrajectoryPoint(
00200 const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk,
00201 const Eigen::Vector3d& snap, double yaw, double yaw_rate,
00202 double yaw_acceleration, Eigen::Quaterniond* orientation,
00203 Eigen::Vector3d* acceleration_body, Eigen::Vector3d* angular_velocity_body,
00204 Eigen::Vector3d* angular_acceleration_body) {
00205 EigenMavStateFromEigenTrajectoryPoint(
00206 acceleration, jerk, snap, yaw, yaw_rate, yaw_acceleration, kGravity,
00207 orientation, acceleration_body, angular_velocity_body,
00208 angular_acceleration_body);
00209 }
00210
00213 inline void EigenMavStateFromEigenTrajectoryPoint(
00214 const EigenTrajectoryPoint& flat_state, double magnitude_of_gravity,
00215 EigenMavState* mav_state) {
00216 assert(mav_state != NULL);
00217 EigenMavStateFromEigenTrajectoryPoint(
00218 flat_state.acceleration_W, flat_state.jerk_W, flat_state.snap_W,
00219 flat_state.getYaw(), flat_state.getYawRate(), flat_state.getYawAcc(),
00220 magnitude_of_gravity, &(mav_state->orientation_W_B),
00221 &(mav_state->acceleration_B), &(mav_state->angular_velocity_B),
00222 &(mav_state->angular_acceleration_B));
00223 mav_state->position_W = flat_state.position_W;
00224 mav_state->velocity_W = flat_state.velocity_W;
00225 }
00226
00232 inline void EigenMavStateFromEigenTrajectoryPoint(
00233 const EigenTrajectoryPoint& flat_state, EigenMavState* mav_state) {
00234 EigenMavStateFromEigenTrajectoryPoint(flat_state, kGravity, mav_state);
00235 }
00236
00237 inline void EigenMavStateFromEigenTrajectoryPoint(
00238 const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk,
00239 const Eigen::Vector3d& snap, double yaw, double yaw_rate,
00240 double yaw_acceleration, double magnitude_of_gravity,
00241 Eigen::Quaterniond* orientation, Eigen::Vector3d* acceleration_body,
00242 Eigen::Vector3d* angular_velocity_body,
00243 Eigen::Vector3d* angular_acceleration_body) {
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268 assert(orientation != nullptr);
00269 assert(acceleration_body != nullptr);
00270 assert(angular_velocity_body != nullptr);
00271 assert(angular_acceleration_body != nullptr);
00272
00273 Eigen::Vector3d xb;
00274 Eigen::Vector3d yb;
00275 Eigen::Vector3d zb(acceleration);
00276
00277 zb[2] += magnitude_of_gravity;
00278 const double thrust = zb.norm();
00279 const double inv_thrust = 1.0 / thrust;
00280 zb = zb * inv_thrust;
00281
00282 yb = zb.cross(Eigen::Vector3d(cos(yaw), sin(yaw), 0.0));
00283 yb.normalize();
00284
00285 xb = yb.cross(zb);
00286
00287 const Eigen::Matrix3d R((Eigen::Matrix3d() << xb, yb, zb).finished());
00288
00289 const Eigen::Vector3d h_w =
00290 inv_thrust * (jerk - double(zb.transpose() * jerk) * zb);
00291
00292 *orientation = Eigen::Quaterniond(R);
00293 *acceleration_body = R.transpose() * zb * thrust;
00294 (*angular_velocity_body)[0] = -h_w.transpose() * yb;
00295 (*angular_velocity_body)[1] = h_w.transpose() * xb;
00296 (*angular_velocity_body)[2] = yaw_rate * zb[2];
00297
00298
00299 const Eigen::Vector3d wcrossz = (*angular_velocity_body).cross(zb);
00300 const Eigen::Vector3d wcrosswcrossz = (*angular_velocity_body).cross(wcrossz);
00301 const Eigen::Vector3d h_a =
00302 inv_thrust * (snap - double(zb.transpose() * snap) * zb) - 2 * wcrossz -
00303 wcrosswcrossz + double(zb.transpose() * wcrosswcrossz) * zb;
00304
00305 (*angular_acceleration_body)[0] = -h_a.transpose() * yb;
00306 (*angular_acceleration_body)[1] = h_a.transpose() * xb;
00307 (*angular_acceleration_body)[2] = yaw_acceleration * zb[2];
00308 }
00309
00310 inline void eigenTrajectoryPointFromMsg(
00311 const trajectory_msgs::MultiDOFJointTrajectoryPoint& msg,
00312 EigenTrajectoryPoint* trajectory_point) {
00313 assert(trajectory_point != NULL);
00314
00315 if (msg.transforms.empty()) {
00316 ROS_ERROR("MultiDofJointTrajectoryPoint is empty.");
00317 return;
00318 }
00319
00320 if (msg.transforms.size() > 1) {
00321 ROS_WARN(
00322 "MultiDofJointTrajectoryPoint message should have one joint, but has "
00323 "%lu. Using first joint.",
00324 msg.transforms.size());
00325 }
00326
00327 trajectory_point->time_from_start_ns = msg.time_from_start.toNSec();
00328 trajectory_point->position_W = vector3FromMsg(msg.transforms[0].translation);
00329 trajectory_point->orientation_W_B =
00330 quaternionFromMsg(msg.transforms[0].rotation);
00331 if (msg.velocities.size() > 0) {
00332 trajectory_point->velocity_W = vector3FromMsg(msg.velocities[0].linear);
00333 trajectory_point->angular_velocity_W =
00334 vector3FromMsg(msg.velocities[0].angular);
00335 } else {
00336 trajectory_point->velocity_W.setZero();
00337 trajectory_point->angular_velocity_W.setZero();
00338 }
00339 if (msg.accelerations.size() > 0) {
00340 trajectory_point->acceleration_W =
00341 vector3FromMsg(msg.accelerations[0].linear);
00342 trajectory_point->angular_acceleration_W =
00343 vector3FromMsg(msg.accelerations[0].angular);
00344 } else {
00345 trajectory_point->acceleration_W.setZero();
00346 trajectory_point->angular_acceleration_W.setZero();
00347 }
00348 trajectory_point->jerk_W.setZero();
00349 trajectory_point->snap_W.setZero();
00350 }
00351
00352 inline void eigenTrajectoryPointVectorFromMsg(
00353 const trajectory_msgs::MultiDOFJointTrajectory& msg,
00354 EigenTrajectoryPointVector* trajectory) {
00355 assert(trajectory != NULL);
00356 trajectory->clear();
00357 for (const auto& msg_point : msg.points) {
00358 EigenTrajectoryPoint point;
00359 eigenTrajectoryPointFromMsg(msg_point, &point);
00360 trajectory->push_back(point);
00361 }
00362 }
00363
00364 inline void eigenTrajectoryPointDequeFromMsg(
00365 const trajectory_msgs::MultiDOFJointTrajectory& msg,
00366 EigenTrajectoryPointDeque* trajectory) {
00367 assert(trajectory != NULL);
00368 trajectory->clear();
00369 for (const auto& msg_point : msg.points) {
00370 EigenTrajectoryPoint point;
00371 eigenTrajectoryPointFromMsg(msg_point, &point);
00372 trajectory->push_back(point);
00373 }
00374 }
00375
00376
00377
00378 inline void msgActuatorsFromEigen(const EigenActuators& actuators,
00379 Actuators* msg) {
00380 assert(msg != NULL);
00381
00382 msg->angles.resize(actuators.angles.size());
00383 for (unsigned int i = 0; i < actuators.angles.size(); ++i) {
00384 msg->angles[i] = actuators.angles[i];
00385 }
00386
00387 msg->angular_velocities.resize(actuators.angular_velocities.size());
00388 for (unsigned int i = 0; i < actuators.angular_velocities.size(); ++i) {
00389 msg->angular_velocities[i] = actuators.angular_velocities[i];
00390 }
00391
00392 msg->normalized.resize(actuators.normalized.size());
00393 for (unsigned int i = 0; i < actuators.normalized.size(); ++i) {
00394 msg->normalized[i] = actuators.normalized[i];
00395 }
00396 }
00397
00398 inline void msgAttitudeThrustFromEigen(
00399 const EigenAttitudeThrust& attitude_thrust, AttitudeThrust* msg) {
00400 assert(msg != NULL);
00401 quaternionEigenToMsg(attitude_thrust.attitude, &msg->attitude);
00402 vectorEigenToMsg(attitude_thrust.thrust, &msg->thrust);
00403 }
00404
00405 inline void msgRateThrustFromEigen(EigenRateThrust& rate_thrust,
00406 RateThrust* msg) {
00407 assert(msg != NULL);
00408 vectorEigenToMsg(rate_thrust.angular_rates, &msg->angular_rates);
00409 vectorEigenToMsg(rate_thrust.thrust, &msg->thrust);
00410 }
00411
00412 inline void msgTorqueThrustFromEigen(EigenTorqueThrust& torque_thrust,
00413 TorqueThrust* msg) {
00414 assert(msg != NULL);
00415 vectorEigenToMsg(torque_thrust.torque, &msg->torque);
00416 vectorEigenToMsg(torque_thrust.thrust, &msg->thrust);
00417 }
00418
00419 inline void msgRollPitchYawrateThrustFromEigen(
00420 const EigenRollPitchYawrateThrust& roll_pitch_yawrate_thrust,
00421 RollPitchYawrateThrust* msg) {
00422 assert(msg != NULL);
00423 msg->roll = roll_pitch_yawrate_thrust.roll;
00424 msg->pitch = roll_pitch_yawrate_thrust.pitch;
00425 msg->yaw_rate = roll_pitch_yawrate_thrust.yaw_rate;
00426 vectorEigenToMsg(roll_pitch_yawrate_thrust.thrust, &msg->thrust);
00427 }
00428
00429 inline void msgOdometryFromEigen(const EigenOdometry& odometry,
00430 nav_msgs::Odometry* msg) {
00431 assert(msg != NULL);
00432
00433 if (odometry.timestamp_ns >= 0) {
00434 msg->header.stamp.fromNSec(odometry.timestamp_ns);
00435 }
00436 pointEigenToMsg(odometry.position_W, &msg->pose.pose.position);
00437 quaternionEigenToMsg(odometry.orientation_W_B, &msg->pose.pose.orientation);
00438
00439 vectorEigenToMsg(odometry.velocity_B, &msg->twist.twist.linear);
00440 vectorEigenToMsg(odometry.angular_velocity_B, &msg->twist.twist.angular);
00441 }
00442
00443
00444 inline void msgPoseStampedFromEigenTrajectoryPoint(
00445 const EigenTrajectoryPoint& trajectory_point,
00446 geometry_msgs::PoseStamped* msg) {
00447 if (trajectory_point.timestamp_ns >= 0) {
00448 msg->header.stamp.fromNSec(trajectory_point.timestamp_ns);
00449 }
00450 pointEigenToMsg(trajectory_point.position_W, &msg->pose.position);
00451 quaternionEigenToMsg(trajectory_point.orientation_W_B,
00452 &msg->pose.orientation);
00453 }
00454
00455 inline void msgMultiDofJointTrajectoryPointFromEigen(
00456 const EigenTrajectoryPoint& trajectory_point,
00457 trajectory_msgs::MultiDOFJointTrajectoryPoint* msg) {
00458 assert(msg != NULL);
00459
00460 msg->time_from_start.fromNSec(trajectory_point.time_from_start_ns);
00461 msg->transforms.resize(1);
00462 msg->velocities.resize(1);
00463 msg->accelerations.resize(1);
00464
00465 vectorEigenToMsg(trajectory_point.position_W,
00466 &msg->transforms[0].translation);
00467 quaternionEigenToMsg(trajectory_point.orientation_W_B,
00468 &msg->transforms[0].rotation);
00469 vectorEigenToMsg(trajectory_point.velocity_W, &msg->velocities[0].linear);
00470 vectorEigenToMsg(trajectory_point.angular_velocity_W,
00471 &msg->velocities[0].angular);
00472 vectorEigenToMsg(trajectory_point.acceleration_W,
00473 &msg->accelerations[0].linear);
00474 }
00475
00476 inline void msgMultiDofJointTrajectoryFromEigen(
00477 const EigenTrajectoryPoint& trajectory_point, const std::string& link_name,
00478 trajectory_msgs::MultiDOFJointTrajectory* msg) {
00479 assert(msg != NULL);
00480 trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
00481 msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &point_msg);
00482
00483 msg->joint_names.clear();
00484 msg->points.clear();
00485 msg->joint_names.push_back(link_name);
00486 msg->points.push_back(point_msg);
00487 }
00488
00489 inline void msgMultiDofJointTrajectoryFromEigen(
00490 const EigenTrajectoryPoint& trajectory_point,
00491 trajectory_msgs::MultiDOFJointTrajectory* msg) {
00492 msgMultiDofJointTrajectoryFromEigen(trajectory_point, "base_link", msg);
00493 }
00494
00495
00496 inline void msgMultiDofJointTrajectoryFromPositionYaw(
00497 const Eigen::Vector3d& position, double yaw,
00498 trajectory_msgs::MultiDOFJointTrajectory* msg) {
00499 assert(msg != NULL);
00500
00501 EigenTrajectoryPoint point;
00502 point.position_W = position;
00503 point.setFromYaw(yaw);
00504
00505 msgMultiDofJointTrajectoryFromEigen(point, msg);
00506 }
00507
00508 inline void msgMultiDofJointTrajectoryFromEigen(
00509 const EigenTrajectoryPointVector& trajectory, const std::string& link_name,
00510 trajectory_msgs::MultiDOFJointTrajectory* msg) {
00511 assert(msg != NULL);
00512
00513 if (trajectory.empty()) {
00514 ROS_ERROR("EigenTrajectoryPointVector is empty.");
00515 return;
00516 }
00517
00518 msg->joint_names.clear();
00519 msg->joint_names.push_back(link_name);
00520 msg->points.clear();
00521
00522 for (const auto& trajectory_point : trajectory) {
00523 trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
00524 msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &point_msg);
00525 msg->points.push_back(point_msg);
00526 }
00527 }
00528
00529 inline void msgMultiDofJointTrajectoryFromEigen(
00530 const EigenTrajectoryPointVector& trajectory,
00531 trajectory_msgs::MultiDOFJointTrajectory* msg) {
00532 msgMultiDofJointTrajectoryFromEigen(trajectory, "base_link", msg);
00533 }
00534
00535 inline void msgMultiDofJointTrajectoryFromEigen(
00536 const EigenTrajectoryPointDeque& trajectory, const std::string& link_name,
00537 trajectory_msgs::MultiDOFJointTrajectory* msg) {
00538 assert(msg != NULL);
00539
00540 if (trajectory.empty()) {
00541 ROS_ERROR("EigenTrajectoryPointVector is empty.");
00542 return;
00543 }
00544
00545 msg->joint_names.clear();
00546 msg->joint_names.push_back(link_name);
00547 msg->points.clear();
00548
00549 for (const auto& trajectory_point : trajectory) {
00550 trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
00551 msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &point_msg);
00552 msg->points.push_back(point_msg);
00553 }
00554 }
00555
00556 inline void msgMultiDofJointTrajectoryFromEigen(
00557 const EigenTrajectoryPointDeque& trajectory,
00558 trajectory_msgs::MultiDOFJointTrajectory* msg) {
00559 msgMultiDofJointTrajectoryFromEigen(trajectory, "base_link", msg);
00560 }
00561
00562 }
00563
00564 #endif // MAV_MSGS_CONVERSIONS_H