23 #ifndef MAV_MSGS_CONVERSIONS_H 24 #define MAV_MSGS_CONVERSIONS_H 26 #include <geometry_msgs/Point.h> 27 #include <geometry_msgs/PoseStamped.h> 28 #include <geometry_msgs/Quaternion.h> 29 #include <geometry_msgs/TransformStamped.h> 30 #include <geometry_msgs/Vector3.h> 31 #include <nav_msgs/Odometry.h> 33 #include <trajectory_msgs/MultiDOFJointTrajectory.h> 35 #include "mav_msgs/Actuators.h" 36 #include "mav_msgs/AttitudeThrust.h" 37 #include "mav_msgs/RateThrust.h" 38 #include "mav_msgs/RollPitchYawrateThrust.h" 39 #include "mav_msgs/TorqueThrust.h" 48 assert(attitude_thrust != NULL);
56 assert(actuators != NULL);
59 actuators->
angles.resize(msg.angles.size());
60 for (
unsigned int i = 0; i < msg.angles.size(); ++i) {
61 actuators->
angles[i] = msg.angles[i];
66 for (
unsigned int i = 0; i < msg.angular_velocities.size(); ++i) {
72 actuators->
normalized.resize(msg.normalized.size());
73 for (
unsigned int i = 0; i < msg.normalized.size(); ++i) {
80 assert(rate_thrust != NULL);
88 assert(torque_thrust != NULL);
95 const RollPitchYawrateThrust& msg,
97 assert(roll_pitch_yawrate_thrust != NULL);
99 roll_pitch_yawrate_thrust->
roll = msg.roll;
100 roll_pitch_yawrate_thrust->
pitch = msg.pitch;
101 roll_pitch_yawrate_thrust->
yaw_rate = msg.yaw_rate;
107 assert(odometry != NULL);
116 Eigen::Map<const Eigen::Matrix<double, 6, 6>>(msg.pose.covariance.data());
118 msg.twist.covariance.data());
122 const geometry_msgs::TransformStamped& msg,
124 assert(trajectory_point != NULL);
135 trajectory_point->
jerk_W.setZero();
136 trajectory_point->
snap_W.setZero();
144 assert(trajectory_point != NULL);
152 trajectory_point->
jerk_W.setZero();
153 trajectory_point->
snap_W.setZero();
157 const geometry_msgs::PoseStamped& msg,
159 assert(trajectory_point != NULL);
190 const Eigen::Vector3d& acceleration,
const Eigen::Vector3d& jerk,
191 const Eigen::Vector3d& snap,
double yaw,
double yaw_rate,
192 double yaw_acceleration,
double magnitude_of_gravity,
193 Eigen::Quaterniond* orientation, Eigen::Vector3d* acceleration_body,
194 Eigen::Vector3d* angular_velocity_body,
195 Eigen::Vector3d* angular_acceleration_body);
200 const Eigen::Vector3d& acceleration,
const Eigen::Vector3d& jerk,
201 const Eigen::Vector3d& snap,
double yaw,
double yaw_rate,
202 double yaw_acceleration, Eigen::Quaterniond* orientation,
203 Eigen::Vector3d* acceleration_body, Eigen::Vector3d* angular_velocity_body,
204 Eigen::Vector3d* angular_acceleration_body) {
206 acceleration, jerk, snap, yaw, yaw_rate, yaw_acceleration,
kGravity,
207 orientation, acceleration_body, angular_velocity_body,
208 angular_acceleration_body);
216 assert(mav_state != NULL);
238 const Eigen::Vector3d& acceleration,
const Eigen::Vector3d& jerk,
239 const Eigen::Vector3d& snap,
double yaw,
double yaw_rate,
240 double yaw_acceleration,
double magnitude_of_gravity,
241 Eigen::Quaterniond* orientation, Eigen::Vector3d* acceleration_body,
242 Eigen::Vector3d* angular_velocity_body,
243 Eigen::Vector3d* angular_acceleration_body) {
268 assert(orientation !=
nullptr);
269 assert(acceleration_body !=
nullptr);
270 assert(angular_velocity_body !=
nullptr);
271 assert(angular_acceleration_body !=
nullptr);
275 Eigen::Vector3d zb(acceleration);
277 zb[2] += magnitude_of_gravity;
278 const double thrust = zb.norm();
279 const double inv_thrust = 1.0 / thrust;
280 zb = zb * inv_thrust;
282 yb = zb.cross(Eigen::Vector3d(cos(yaw), sin(yaw), 0.0));
287 const Eigen::Matrix3d R((Eigen::Matrix3d() << xb, yb, zb).finished());
289 const Eigen::Vector3d h_w =
290 inv_thrust * (jerk - double(zb.transpose() * jerk) * zb);
292 *orientation = Eigen::Quaterniond(R);
293 *acceleration_body = R.transpose() * zb * thrust;
294 (*angular_velocity_body)[0] = -h_w.transpose() * yb;
295 (*angular_velocity_body)[1] = h_w.transpose() * xb;
296 (*angular_velocity_body)[2] = yaw_rate * zb[2];
299 const Eigen::Vector3d wcrossz = (*angular_velocity_body).cross(zb);
300 const Eigen::Vector3d wcrosswcrossz = (*angular_velocity_body).cross(wcrossz);
301 const Eigen::Vector3d h_a =
302 inv_thrust * (snap - double(zb.transpose() * snap) * zb) - 2 * wcrossz -
303 wcrosswcrossz + double(zb.transpose() * wcrosswcrossz) * zb;
305 (*angular_acceleration_body)[0] = -h_a.transpose() * yb;
306 (*angular_acceleration_body)[1] = h_a.transpose() * xb;
307 (*angular_acceleration_body)[2] = yaw_acceleration * zb[2];
311 const trajectory_msgs::MultiDOFJointTrajectoryPoint& msg,
313 assert(trajectory_point != NULL);
315 if (msg.transforms.empty()) {
316 ROS_ERROR(
"MultiDofJointTrajectoryPoint is empty.");
320 if (msg.transforms.size() > 1) {
322 "MultiDofJointTrajectoryPoint message should have one joint, but has " 323 "%lu. Using first joint.",
324 msg.transforms.size());
331 if (msg.velocities.size() > 0) {
339 if (msg.accelerations.size() > 0) {
348 trajectory_point->
jerk_W.setZero();
349 trajectory_point->
snap_W.setZero();
353 const trajectory_msgs::MultiDOFJointTrajectory& msg,
354 EigenTrajectoryPointVector* trajectory) {
355 assert(trajectory != NULL);
357 for (
const auto& msg_point : msg.points) {
360 trajectory->push_back(point);
365 const trajectory_msgs::MultiDOFJointTrajectory& msg,
366 EigenTrajectoryPointDeque* trajectory) {
367 assert(trajectory != NULL);
369 for (
const auto& msg_point : msg.points) {
372 trajectory->push_back(point);
382 msg->angles.resize(actuators.
angles.size());
383 for (
unsigned int i = 0; i < actuators.
angles.size(); ++i) {
384 msg->angles[i] = actuators.
angles[i];
392 msg->normalized.resize(actuators.
normalized.size());
393 for (
unsigned int i = 0; i < actuators.
normalized.size(); ++i) {
421 RollPitchYawrateThrust* msg) {
423 msg->roll = roll_pitch_yawrate_thrust.
roll;
424 msg->pitch = roll_pitch_yawrate_thrust.
pitch;
425 msg->yaw_rate = roll_pitch_yawrate_thrust.
yaw_rate;
430 nav_msgs::Odometry* msg) {
446 geometry_msgs::PoseStamped* msg) {
448 msg->header.stamp.fromNSec(trajectory_point.
timestamp_ns);
452 &msg->pose.orientation);
457 trajectory_msgs::MultiDOFJointTrajectoryPoint* msg) {
461 msg->transforms.resize(1);
462 msg->velocities.resize(1);
463 msg->accelerations.resize(1);
466 &msg->transforms[0].translation);
468 &msg->transforms[0].rotation);
471 &msg->velocities[0].angular);
473 &msg->accelerations[0].linear);
478 trajectory_msgs::MultiDOFJointTrajectory* msg) {
480 trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
483 msg->joint_names.clear();
485 msg->joint_names.push_back(link_name);
486 msg->points.push_back(point_msg);
491 trajectory_msgs::MultiDOFJointTrajectory* msg) {
497 const Eigen::Vector3d& position,
double yaw,
498 trajectory_msgs::MultiDOFJointTrajectory* msg) {
509 const EigenTrajectoryPointVector& trajectory,
const std::string& link_name,
510 trajectory_msgs::MultiDOFJointTrajectory* msg) {
513 if (trajectory.empty()) {
514 ROS_ERROR(
"EigenTrajectoryPointVector is empty.");
518 msg->joint_names.clear();
519 msg->joint_names.push_back(link_name);
522 for (
const auto& trajectory_point : trajectory) {
523 trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
525 msg->points.push_back(point_msg);
530 const EigenTrajectoryPointVector& trajectory,
531 trajectory_msgs::MultiDOFJointTrajectory* msg) {
536 const EigenTrajectoryPointDeque& trajectory,
const std::string& link_name,
537 trajectory_msgs::MultiDOFJointTrajectory* msg) {
540 if (trajectory.empty()) {
541 ROS_ERROR(
"EigenTrajectoryPointVector is empty.");
545 msg->joint_names.clear();
546 msg->joint_names.push_back(link_name);
549 for (
const auto& trajectory_point : trajectory) {
550 trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
552 msg->points.push_back(point_msg);
557 const EigenTrajectoryPointDeque& trajectory,
558 trajectory_msgs::MultiDOFJointTrajectory* msg) {
564 #endif // MAV_MSGS_CONVERSIONS_H Eigen::Vector3d acceleration_W
void quaternionEigenToMsg(const Eigen::Quaterniond &eigen, geometry_msgs::Quaternion *msg)
void eigenTrajectoryPointFromMsg(const trajectory_msgs::MultiDOFJointTrajectoryPoint &msg, EigenTrajectoryPoint *trajectory_point)
void eigenTrajectoryPointDequeFromMsg(const trajectory_msgs::MultiDOFJointTrajectory &msg, EigenTrajectoryPointDeque *trajectory)
void msgOdometryFromEigen(const EigenOdometry &odometry, nav_msgs::Odometry *msg)
Eigen::VectorXd normalized
Eigen::Vector3d position_W
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int64_t timestamp_ns
void msgRollPitchYawrateThrustFromEigen(const EigenRollPitchYawrateThrust &roll_pitch_yawrate_thrust, RollPitchYawrateThrust *msg)
Eigen::Vector3d vector3FromMsg(const geometry_msgs::Vector3 &msg)
Eigen::Vector3d velocity_B
void msgMultiDofJointTrajectoryPointFromEigen(const EigenTrajectoryPoint &trajectory_point, trajectory_msgs::MultiDOFJointTrajectoryPoint *msg)
Eigen::Matrix< double, 6, 6 > twist_covariance_
void msgMultiDofJointTrajectoryFromEigen(const EigenTrajectoryPoint &trajectory_point, const std::string &link_name, trajectory_msgs::MultiDOFJointTrajectory *msg)
Eigen::Matrix< double, 6, 6 > pose_covariance_
Eigen::Quaterniond orientation_W_B
void EigenMavStateFromEigenTrajectoryPoint(const Eigen::Vector3d &acceleration, const Eigen::Vector3d &jerk, const Eigen::Vector3d &snap, double yaw, double yaw_rate, double yaw_acceleration, double magnitude_of_gravity, Eigen::Quaterniond *orientation, Eigen::Vector3d *acceleration_body, Eigen::Vector3d *angular_velocity_body, Eigen::Vector3d *angular_acceleration_body)
Computes the MAV state (position, velocity, attitude, angular velocity, angular acceleration) from th...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d angular_rates
void eigenOdometryFromMsg(const nav_msgs::Odometry &msg, EigenOdometry *odometry)
Eigen::Vector3d velocity_W
void eigenTrajectoryPointFromPoseMsg(const geometry_msgs::Pose &msg, EigenTrajectoryPoint *trajectory_point)
Eigen::Vector3d angular_velocity_W
Eigen::VectorXd angular_velocities
void eigenTrajectoryPointVectorFromMsg(const trajectory_msgs::MultiDOFJointTrajectory &msg, EigenTrajectoryPointVector *trajectory)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int64_t timestamp_ns
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::VectorXd angles
void eigenAttitudeThrustFromMsg(const AttitudeThrust &msg, EigenAttitudeThrust *attitude_thrust)
double getYawRate() const
Eigen::Quaterniond orientation_W_B
Eigen::Vector3d position_W
void msgPoseStampedFromEigenTrajectoryPoint(const EigenTrajectoryPoint &trajectory_point, geometry_msgs::PoseStamped *msg)
Eigen::Vector3d angular_acceleration_B
int64_t time_from_start_ns
void eigenTrajectoryPointFromTransformMsg(const geometry_msgs::TransformStamped &msg, EigenTrajectoryPoint *trajectory_point)
void eigenRateThrustFromMsg(const RateThrust &msg, EigenRateThrust *rate_thrust)
void msgTorqueThrustFromEigen(EigenTorqueThrust &torque_thrust, TorqueThrust *msg)
void eigenActuatorsFromMsg(const Actuators &msg, EigenActuators *actuators)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Quaterniond attitude
Eigen::Vector3d position_W
void vectorEigenToMsg(const Eigen::Vector3d &eigen, geometry_msgs::Vector3 *msg)
Container holding the state of a MAV: position, velocity, attitude and angular velocity. In addition, holds the acceleration expressed in body coordinates, which is what the accelerometer usually measures.
void msgMultiDofJointTrajectoryFromPositionYaw(const Eigen::Vector3d &position, double yaw, trajectory_msgs::MultiDOFJointTrajectory *msg)
Eigen::Vector3d angular_velocity_B
Eigen::Vector3d angular_acceleration_W
Eigen::Quaterniond quaternionFromMsg(const geometry_msgs::Quaternion &msg)
Eigen::Quaterniond orientation_W_B
Eigen::Vector3d vector3FromPointMsg(const geometry_msgs::Point &msg)
void msgAttitudeThrustFromEigen(const EigenAttitudeThrust &attitude_thrust, AttitudeThrust *msg)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d torque
void eigenTorqueThrustFromMsg(const TorqueThrust &msg, EigenTorqueThrust *torque_thrust)
Eigen::Vector3d velocity_W
void setFromYaw(double yaw)
void eigenRollPitchYawrateThrustFromMsg(const RollPitchYawrateThrust &msg, EigenRollPitchYawrateThrust *roll_pitch_yawrate_thrust)
Eigen::Vector3d angular_velocity_B
void pointEigenToMsg(const Eigen::Vector3d &eigen, geometry_msgs::Point *msg)
Eigen::Vector3d acceleration_B
void msgRateThrustFromEigen(EigenRateThrust &rate_thrust, RateThrust *msg)
void msgActuatorsFromEigen(const EigenActuators &actuators, Actuators *msg)