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);
229 Eigen::Matrix3d R = trajectory_point.
orientation_W_B.toRotationMatrix();
234 Eigen::Vector3d(0.0, 0.0, magnitude_of_gravity));
254 const Eigen::Vector3d& acceleration,
const Eigen::Vector3d& jerk,
255 const Eigen::Vector3d& snap,
double yaw,
double yaw_rate,
256 double yaw_acceleration,
double magnitude_of_gravity,
257 Eigen::Quaterniond* orientation, Eigen::Vector3d* acceleration_body,
258 Eigen::Vector3d* angular_velocity_body,
259 Eigen::Vector3d* angular_acceleration_body) {
284 assert(orientation !=
nullptr);
285 assert(acceleration_body !=
nullptr);
286 assert(angular_velocity_body !=
nullptr);
287 assert(angular_acceleration_body !=
nullptr);
291 Eigen::Vector3d zb(acceleration);
293 zb[2] += magnitude_of_gravity;
294 const double thrust = zb.norm();
295 const double inv_thrust = 1.0 / thrust;
296 zb = zb * inv_thrust;
298 yb = zb.cross(Eigen::Vector3d(cos(yaw), sin(yaw), 0.0));
303 const Eigen::Matrix3d R((Eigen::Matrix3d() << xb, yb, zb).finished());
305 const Eigen::Vector3d h_w =
306 inv_thrust * (jerk - double(zb.transpose() * jerk) * zb);
308 *orientation = Eigen::Quaterniond(R);
309 *acceleration_body = R.transpose() * zb * thrust;
310 (*angular_velocity_body)[0] = -h_w.transpose() * yb;
311 (*angular_velocity_body)[1] = h_w.transpose() * xb;
312 (*angular_velocity_body)[2] = yaw_rate * zb[2];
315 const Eigen::Vector3d wcrossz = (*angular_velocity_body).cross(zb);
316 const Eigen::Vector3d wcrosswcrossz = (*angular_velocity_body).cross(wcrossz);
317 const Eigen::Vector3d h_a =
318 inv_thrust * (snap - double(zb.transpose() * snap) * zb) - 2 * wcrossz -
319 wcrosswcrossz + double(zb.transpose() * wcrosswcrossz) * zb;
321 (*angular_acceleration_body)[0] = -h_a.transpose() * yb;
322 (*angular_acceleration_body)[1] = h_a.transpose() * xb;
323 (*angular_acceleration_body)[2] = yaw_acceleration * zb[2];
327 const trajectory_msgs::MultiDOFJointTrajectoryPoint& msg,
329 assert(trajectory_point != NULL);
331 if (msg.transforms.empty()) {
332 ROS_ERROR(
"MultiDofJointTrajectoryPoint is empty.");
336 if (msg.transforms.size() > 1) {
338 "MultiDofJointTrajectoryPoint message should have one joint, but has " 339 "%lu. Using first joint.",
340 msg.transforms.size());
347 if (msg.velocities.size() > 0) {
355 if (msg.accelerations.size() > 0) {
364 trajectory_point->
jerk_W.setZero();
365 trajectory_point->
snap_W.setZero();
369 const trajectory_msgs::MultiDOFJointTrajectory& msg,
370 EigenTrajectoryPointVector* trajectory) {
371 assert(trajectory != NULL);
373 for (
const auto& msg_point : msg.points) {
376 trajectory->push_back(point);
381 const trajectory_msgs::MultiDOFJointTrajectory& msg,
382 EigenTrajectoryPointDeque* trajectory) {
383 assert(trajectory != NULL);
385 for (
const auto& msg_point : msg.points) {
388 trajectory->push_back(point);
398 msg->angles.resize(actuators.
angles.size());
399 for (
unsigned int i = 0; i < actuators.
angles.size(); ++i) {
400 msg->angles[i] = actuators.
angles[i];
408 msg->normalized.resize(actuators.
normalized.size());
409 for (
unsigned int i = 0; i < actuators.
normalized.size(); ++i) {
437 RollPitchYawrateThrust* msg) {
439 msg->roll = roll_pitch_yawrate_thrust.
roll;
440 msg->pitch = roll_pitch_yawrate_thrust.
pitch;
441 msg->yaw_rate = roll_pitch_yawrate_thrust.
yaw_rate;
446 nav_msgs::Odometry* msg) {
462 geometry_msgs::PoseStamped* msg) {
464 msg->header.stamp.fromNSec(trajectory_point.
timestamp_ns);
468 &msg->pose.orientation);
473 trajectory_msgs::MultiDOFJointTrajectoryPoint* msg) {
477 msg->transforms.resize(1);
478 msg->velocities.resize(1);
479 msg->accelerations.resize(1);
482 &msg->transforms[0].translation);
484 &msg->transforms[0].rotation);
487 &msg->velocities[0].angular);
489 &msg->accelerations[0].linear);
491 &msg->accelerations[0].angular);
496 trajectory_msgs::MultiDOFJointTrajectory* msg) {
498 trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
501 msg->joint_names.clear();
503 msg->joint_names.push_back(link_name);
504 msg->points.push_back(point_msg);
509 trajectory_msgs::MultiDOFJointTrajectory* msg) {
515 const Eigen::Vector3d& position,
double yaw,
516 trajectory_msgs::MultiDOFJointTrajectory* msg) {
527 const EigenTrajectoryPointVector& trajectory,
const std::string& link_name,
528 trajectory_msgs::MultiDOFJointTrajectory* msg) {
531 if (trajectory.empty()) {
532 ROS_ERROR(
"EigenTrajectoryPointVector is empty.");
536 msg->joint_names.clear();
537 msg->joint_names.push_back(link_name);
540 for (
const auto& trajectory_point : trajectory) {
541 trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
543 msg->points.push_back(point_msg);
548 const EigenTrajectoryPointVector& trajectory,
549 trajectory_msgs::MultiDOFJointTrajectory* msg) {
554 const EigenTrajectoryPointDeque& trajectory,
const std::string& link_name,
555 trajectory_msgs::MultiDOFJointTrajectory* msg) {
558 if (trajectory.empty()) {
559 ROS_ERROR(
"EigenTrajectoryPointVector is empty.");
563 msg->joint_names.clear();
564 msg->joint_names.push_back(link_name);
567 for (
const auto& trajectory_point : trajectory) {
568 trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg;
570 msg->points.push_back(point_msg);
575 const EigenTrajectoryPointDeque& trajectory,
576 trajectory_msgs::MultiDOFJointTrajectory* msg) {
582 #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...
MavActuation degrees_of_freedom
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)