plugin: setpoint_accel #319: use eigen frame transform.

I don't think that PX4 support any other frame than LOCAL_NED.
So i removed comment.

Also style fix in setpoint_velocity.

mavros/src/plugins/setpoint_accel.cpp

@@ -18,6 +18,7 @@
 #include <mavros/utils.h>
 #include <mavros/mavros_plugin.h>
 #include <mavros/setpoint_mixin.h>
+#include <eigen_conversions/eigen_msg.h>
 #include <pluginlib/class_list_macros.h>
 
 #include <geometry_msgs/Vector3Stamped.h>
@@ -66,20 +67,19 @@ class SetpointAccelerationPlugin : public MavRosPlugin,
  *
  * @warning Send only AFX AFY AFZ. ENU frame.
  */
- void send_setpoint_acceleration(const ros::Time &stamp, double afx, double afy, double afz) {
- /**
- * Documentation start from bit 1 instead 0.
+ void send_setpoint_acceleration(const ros::Time &stamp, Eigen::Vector3d &accel_enu) {
+ /* Documentation start from bit 1 instead 0.
  * Ignore position and velocity vectors, yaw and yaw rate
  */
  uint16_t ignore_all_except_a_xyz = (3 << 10) | (7 << 3) | (7 << 0);
 
  if (send_force)
  ignore_all_except_a_xyz |= (1 << 9);
 
- auto accel = UAS::transform_frame_enu_ned_xyz(afx, afy, afz);
+ auto accel = UAS::transform_frame_enu_ned(accel_enu);
 
  set_position_target_local_ned(stamp.toNSec() / 1000000,
- MAV_FRAME_LOCAL_NED, // TODO: use enum on lib
+ MAV_FRAME_LOCAL_NED,
  ignore_all_except_a_xyz,
  0.0, 0.0, 0.0,
  0.0, 0.0, 0.0,
@@ -90,10 +90,10 @@ class SetpointAccelerationPlugin : public MavRosPlugin,
  /* -*- callbacks -*- */
 
  void accel_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) {
- send_setpoint_acceleration(req->header.stamp,
- req->vector.x,
-  req->vector.y,
-  req->vector.z);
+ Eigen::Vector3d accel_enu;
+
+ tf::vectorMsgToEigen(req->vector, accel_enu);
+ send_setpoint_acceleration(req->header.stamp, accel_enu);
  }
 };
 }; // namespace mavplugin

mavros/src/plugins/setpoint_velocity.cpp

@@ -62,14 +62,14 @@ class SetpointVelocityPlugin : public MavRosPlugin,
  *
  * @warning Send only VX VY VZ. ENU frame.
  */
- void send_setpoint_velocity(const ros::Time &stamp, Eigen::Vector3d &linear_enu, double yaw_rate) {
+ void send_setpoint_velocity(const ros::Time &stamp, Eigen::Vector3d &vel_enu, double yaw_rate) {
  /**
  * Documentation start from bit 1 instead 0;
  * Ignore position and accel vectors, yaw.
  */
  uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0);
 
- auto vel = UAS::transform_frame_enu_ned(linear_enu);
+ auto vel = UAS::transform_frame_enu_ned(vel_enu);
  auto yr = UAS::transform_frame_enu_ned(Eigen::Vector3d(0.0, 0.0, yaw_rate));
 
  set_position_target_local_ned(stamp.toNSec() / 1000000,
@@ -84,10 +84,10 @@ class SetpointVelocityPlugin : public MavRosPlugin,
  /* -*- callbacks -*- */
 
  void vel_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
- Eigen::Vector3d linear;
+ Eigen::Vector3d vel_enu;
 
- tf::vectorMsgToEigen(req->twist.linear, linear);
- send_setpoint_velocity(req->header.stamp, linear,
+ tf::vectorMsgToEigen(req->twist.linear, vel_enu);
+ send_setpoint_velocity(req->header.stamp, vel_enu,
  req->twist.angular.z);
  }
 };