From f2b3c6b48bebe176e76baa326b3bb9267ff38a8f Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 7 Jul 2015 11:41:35 +0300 Subject: [PATCH 01/20] plugin: command: Add command broadcasting support. --- mavros/src/plugins/command.cpp | 80 ++++++++++++++++++++++------------ mavros/srv/CommandInt.srv | 2 + mavros/srv/CommandLong.srv | 2 + 3 files changed, 56 insertions(+), 28 deletions(-) diff --git a/mavros/src/plugins/command.cpp b/mavros/src/plugins/command.cpp index 3d7d570f5..fe5bcd626 100644 --- a/mavros/src/plugins/command.cpp +++ b/mavros/src/plugins/command.cpp @@ -48,8 +48,8 @@ class CommandTransaction { class CommandPlugin : public MavRosPlugin { public: CommandPlugin() : - cmd_nh("~cmd"), uas(nullptr), + cmd_nh("~cmd"), use_comp_id_system_control(false), ACK_TIMEOUT_DT(ACK_TIMEOUT_MS / 1000.0) { }; @@ -110,7 +110,7 @@ class CommandPlugin : public MavRosPlugin { return; } - ROS_WARN_THROTTLE_NAMED(10, "cmd", "Unexpected command %u, result %u", + ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Unexpected command %u, result %u", ack.command, ack.result); } @@ -128,41 +128,47 @@ class CommandPlugin : public MavRosPlugin { * * NOTE: success is bool in messages, but has unsigned char type in C++ */ - bool send_command_long_and_wait(uint16_t command, uint8_t confirmation, + bool send_command_long_and_wait(bool broadcast, + uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, - unsigned char &success, uint8_t &result) { + unsigned char &success, uint8_t &result) + { unique_lock lock(mutex); /* check transactions */ for (auto it = ack_waiting_list.cbegin(); it != ack_waiting_list.cend(); it++) if ((*it)->expected_command == command) { - ROS_WARN_THROTTLE_NAMED(10, "cmd", "Command %u alredy in progress", command); + ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Command %u alredy in progress", command); return false; } - //! @note APM always send COMMAND_ACK, while PX4 never. + /** + * @note APM always send COMMAND_ACK, while PX4 never. + * Don't expect any ACK in broadcast mode. + */ bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega()) && !uas->is_px4(); - if (is_ack_required) + if (is_ack_required && !broadcast) ack_waiting_list.push_back(new CommandTransaction(command)); - command_long(command, confirmation, + command_long(broadcast, + command, confirmation, param1, param2, param3, param4, param5, param6, param7); - if (is_ack_required) { + if (is_ack_required && !broadcast) { auto it = ack_waiting_list.begin(); for (; it != ack_waiting_list.end(); it++) if ((*it)->expected_command == command) break; if (it == ack_waiting_list.end()) { - ROS_ERROR_NAMED("cmd", "CommandTransaction not found for %u", command); + ROS_ERROR_NAMED("cmd", "CMD: CommandTransaction not found for %u", command); return false; } @@ -187,7 +193,8 @@ class CommandPlugin : public MavRosPlugin { /** * Common function for COMMAND_INT service callbacks. */ - bool send_command_int(uint8_t frame, uint16_t command, + bool send_command_int(bool broadcast, + uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, @@ -197,7 +204,8 @@ class CommandPlugin : public MavRosPlugin { /* Note: seems that COMMAND_INT don't produce COMMAND_ACK * so wait don't needed. */ - command_int(frame, command, current, autocontinue, + command_int(broadcast, + frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); @@ -208,20 +216,25 @@ class CommandPlugin : public MavRosPlugin { /* -*- low-level send -*- */ - void command_long(uint16_t command, uint8_t confirmation, + void command_long(bool broadcast, + uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, - float param7) { + float param7) + { mavlink_message_t msg; - const uint8_t tgt_comp_id = (use_comp_id_system_control) ? + const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system(); + const uint8_t tgt_comp_id = (broadcast) ? 0 : + (use_comp_id_system_control) ? MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component(); + const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation; mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg, - uas->get_tgt_system(), + tgt_sys_id, tgt_comp_id, command, - confirmation, + confirmation_fixed, param1, param2, param3, param4, param5, param6, @@ -229,18 +242,22 @@ class CommandPlugin : public MavRosPlugin { UAS_FCU(uas)->send_message(&msg); } - void command_int(uint8_t frame, uint16_t command, + void command_int(bool broadcast, + uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, - float z) { + float z) + { mavlink_message_t msg; - const uint8_t tgt_comp_id = (use_comp_id_system_control) ? + const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system(); + const uint8_t tgt_comp_id = (broadcast) ? 0 : + (use_comp_id_system_control) ? MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component(); mavlink_msg_command_int_pack_chan(UAS_PACK_CHAN(uas), &msg, - uas->get_tgt_system(), + tgt_sys_id, tgt_comp_id, frame, command, @@ -256,7 +273,8 @@ class CommandPlugin : public MavRosPlugin { bool command_long_cb(mavros::CommandLong::Request &req, mavros::CommandLong::Response &res) { - return send_command_long_and_wait(req.command, req.confirmation, + return send_command_long_and_wait(req.broadcast, + req.command, req.confirmation, req.param1, req.param2, req.param3, req.param4, req.param5, req.param6, @@ -266,7 +284,8 @@ class CommandPlugin : public MavRosPlugin { bool command_int_cb(mavros::CommandInt::Request &req, mavros::CommandInt::Response &res) { - return send_command_int(req.frame, req.command, + return send_command_int(req.broadcast, + req.frame, req.command, req.current, req.autocontinue, req.param1, req.param2, req.param3, req.param4, @@ -276,7 +295,8 @@ class CommandPlugin : public MavRosPlugin { bool arming_cb(mavros::CommandBool::Request &req, mavros::CommandBool::Response &res) { - return send_command_long_and_wait(MAV_CMD_COMPONENT_ARM_DISARM, 1, + return send_command_long_and_wait(false, + MAV_CMD_COMPONENT_ARM_DISARM, 1, (req.value) ? 1.0 : 0.0, 0, 0, 0, 0, 0, 0, res.success, res.result); @@ -284,7 +304,8 @@ class CommandPlugin : public MavRosPlugin { bool set_home_cb(mavros::CommandHome::Request &req, mavros::CommandHome::Response &res) { - return send_command_long_and_wait(MAV_CMD_DO_SET_HOME, 1, + return send_command_long_and_wait(false, + MAV_CMD_DO_SET_HOME, 1, (req.current_gps) ? 1.0 : 0.0, 0, 0, 0, req.latitude, req.longitude, req.altitude, res.success, res.result); @@ -292,7 +313,8 @@ class CommandPlugin : public MavRosPlugin { bool takeoff_cb(mavros::CommandTOL::Request &req, mavros::CommandTOL::Response &res) { - return send_command_long_and_wait(MAV_CMD_NAV_TAKEOFF, 1, + return send_command_long_and_wait(false, + MAV_CMD_NAV_TAKEOFF, 1, req.min_pitch, 0, 0, req.yaw, @@ -302,7 +324,8 @@ class CommandPlugin : public MavRosPlugin { bool land_cb(mavros::CommandTOL::Request &req, mavros::CommandTOL::Response &res) { - return send_command_long_and_wait(MAV_CMD_NAV_LAND, 1, + return send_command_long_and_wait(false, + MAV_CMD_NAV_LAND, 1, 0, 0, 0, req.yaw, req.latitude, req.longitude, req.altitude, @@ -311,7 +334,8 @@ class CommandPlugin : public MavRosPlugin { bool guided_cb(mavros::CommandBool::Request &req, mavros::CommandBool::Response &res) { - return send_command_long_and_wait(MAV_CMD_NAV_GUIDED_ENABLE, 1, + return send_command_long_and_wait(false, + MAV_CMD_NAV_GUIDED_ENABLE, 1, (req.value) ? 1.0 : 0.0, 0, 0, 0, 0, 0, 0, res.success, res.result); diff --git a/mavros/srv/CommandInt.srv b/mavros/srv/CommandInt.srv index da55298bb..f2242c757 100644 --- a/mavros/srv/CommandInt.srv +++ b/mavros/srv/CommandInt.srv @@ -1,5 +1,7 @@ # Generic COMMAND_INT +bool broadcast # send this command in broadcast mode + uint8 frame uint16 command uint8 current diff --git a/mavros/srv/CommandLong.srv b/mavros/srv/CommandLong.srv index ebca0ee48..97fb2f252 100644 --- a/mavros/srv/CommandLong.srv +++ b/mavros/srv/CommandLong.srv @@ -23,6 +23,8 @@ uint16 CMD_COMPONENT_ARM_DISARM = 400 uint16 CMD_START_RX_PAIR = 500 uint16 CMD_REQUEST_AUTOPILOT_CAPABILITIES = 520 +bool broadcast # send this command in broadcast mode + uint16 command uint8 confirmation float32 param1 From 3b4305c4fb2b559a6e494b148cac4584ac00d1ca Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Thu, 9 Jul 2015 14:41:12 +0300 Subject: [PATCH 02/20] lib: Add UAS::synchonized_header() --- mavros/include/mavros/mavros_uas.h | 39 +++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h index 4182a4783..089713aae 100644 --- a/mavros/include/mavros/mavros_uas.h +++ b/mavros/include/mavros/mavros_uas.h @@ -7,7 +7,7 @@ * @{ */ /* - * Copyright 2014 Vladimir Ermakov. + * Copyright 2014,2015 Vladimir Ermakov. * * This file is part of the mavros package and subject to the license terms * in the top-level LICENSE file of the mavros repository. @@ -216,6 +216,33 @@ class UAS { uint64_t get_capabilities(); void update_capabilities(bool known, uint64_t caps = 0); + /** + * @brief Compute FCU message time from time_boot_ms or time_usec field + * + * Uses time_offset for calculation + * + * @return FCU time if it is known else current wall time. + */ + ros::Time synchronise_stamp(uint32_t time_boot_ms); + ros::Time synchronise_stamp(uint64_t time_usec); + + /** + * @brief Create message header from time_boot_ms or time_usec stamps and frame_id. + * + * Setting frame_id and stamp are pretty common, this little helper should reduce LOC. + * + * @param[in] frame_id frame for header + * @param[in] time_stamp mavlink message time + * @return Header with syncronized stamp and frame id + */ + template + inline std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp) { + std_msgs::Header out; + out.frame_id = frame_id; + out.stamp = synchronise_stamp(time_stamp); + return out; + } + /* -*- utils -*- */ /** @@ -272,16 +299,6 @@ class UAS { */ bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode); - /** - * @brief Compute FCU message time from time_boot_ms or time_usec field - * - * Uses time_offset for calculation - * - * @return FCU time if it is known else current wall time. - */ - ros::Time synchronise_stamp(uint32_t time_boot_ms); - ros::Time synchronise_stamp(uint64_t time_usec); - /** * @brief Represent MAV_AUTOPILOT as string */ From 8ccd073a20b379c117a63ddb79e89beac83371ed Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Thu, 9 Jul 2015 18:35:19 +0300 Subject: [PATCH 03/20] plugin: local_position #71 #319: port to TF2 and Eigen --- mavros/src/plugins/local_position.cpp | 63 +++++++++++++++------------ 1 file changed, 34 insertions(+), 29 deletions(-) diff --git a/mavros/src/plugins/local_position.cpp b/mavros/src/plugins/local_position.cpp index aa2a00eda..5c2ba9336 100644 --- a/mavros/src/plugins/local_position.cpp +++ b/mavros/src/plugins/local_position.cpp @@ -17,9 +17,11 @@ #include #include +#include +#include -#include #include +#include namespace mavplugin { /** @@ -31,16 +33,19 @@ class LocalPositionPlugin : public MavRosPlugin { LocalPositionPlugin() : lp_nh("~local_position"), uas(nullptr), - send_tf(false) + tf_send(false) { }; void initialize(UAS &uas_) { uas = &uas_; - lp_nh.param("send_tf", send_tf, true); - lp_nh.param("frame_id", frame_id, "local_origin"); - lp_nh.param("child_frame_id", child_frame_id, "fcu"); + // general params + lp_nh.param("frame_id", frame_id, "fcu"); + // tf subsection + lp_nh.param("tf/send", tf_send, true); + lp_nh.param("tf/frame_id", tf_frame_id, "local_origin"); + lp_nh.param("tf/child_frame_id", tf_child_frame_id, "fcu"); local_position = lp_nh.advertise("local", 10); } @@ -56,43 +61,43 @@ class LocalPositionPlugin : public MavRosPlugin { UAS *uas; ros::Publisher local_position; - tf::TransformBroadcaster tf_broadcaster; + tf2_ros::TransformBroadcaster tf2_broadcaster; - std::string frame_id; //!< origin for TF - std::string child_frame_id; //!< frame for TF and Pose - bool send_tf; + std::string frame_id; //!< frame for Pose + std::string tf_frame_id; //!< origin for TF + std::string tf_child_frame_id; //!< frame for TF + bool tf_send; void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { mavlink_local_position_ned_t pos_ned; mavlink_msg_local_position_ned_decode(msg, &pos_ned); - ROS_DEBUG_THROTTLE_NAMED(10, "position", "Local position NED: boot_ms:%06d " - "position:(%1.3f %1.3f %1.3f) speed:(%1.3f %1.3f %1.3f)", - pos_ned.time_boot_ms, - pos_ned.x, pos_ned.y, pos_ned.z, - pos_ned.vx, pos_ned.vy, pos_ned.vz); + auto position = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z)); + auto orientation = uas->get_attitude_orientation(); - tf::Transform transform; + auto pose = boost::make_shared(); - auto position = UAS::transform_frame_ned_enu_xyz(pos_ned.x, pos_ned.y, pos_ned.z); + pose->header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms); - transform.setOrigin(position); - transform.setRotation(uas->get_attitude_orientation()); + tf::pointEigenToMsg(position, pose->pose.position); - auto pose = boost::make_shared(); + // XXX FIX ME #319 + tf::quaternionTFToMsg(orientation, pose->pose.orientation); + + local_position.publish(pose); - tf::poseTFToMsg(transform, pose->pose); - pose->header.frame_id = frame_id; - pose->header.stamp = uas->synchronise_stamp(pos_ned.time_boot_ms); + if (tf_send) { + geometry_msgs::TransformStamped transform; - if (send_tf) - tf_broadcaster.sendTransform( - tf::StampedTransform( - transform, - pose->header.stamp, - frame_id, child_frame_id)); + transform.header.stamp = pose->header.stamp; + transform.header.frame_id = tf_frame_id; + transform.child_frame_id = tf_child_frame_id; - local_position.publish(pose); + transform.transform.rotation = pose->pose.orientation; + tf::vectorEigenToMsg(position, transform.transform.translation); + + tf2_broadcaster.sendTransform(transform); + } } }; }; // namespace mavplugin From 48e371ae5bcf372ef115a06c2c2516b2859b3507 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Thu, 9 Jul 2015 18:42:52 +0300 Subject: [PATCH 04/20] plugin: safety_area #319: Change transform_frame() --- mavros/src/plugins/safety_area.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavros/src/plugins/safety_area.cpp b/mavros/src/plugins/safety_area.cpp index ef5f8ed8a..e42507a6d 100644 --- a/mavros/src/plugins/safety_area.cpp +++ b/mavros/src/plugins/safety_area.cpp @@ -109,8 +109,8 @@ class SafetyAreaPlugin : public MavRosPlugin { p1x, p1y, p1z, p2x, p2y, p2z); - auto p1 = UAS::transform_frame_enu_ned_xyz(p1x, p1y, p1z); - auto p2 = UAS::transform_frame_enu_ned_xyz(p2x, p2y, p2z); + auto p1 = UAS::transform_frame_enu_ned(Eigen::Vector3d(p1x, p1y, p1z)); + auto p2 = UAS::transform_frame_enu_ned(Eigen::Vector3d(p2x, p2y, p2z)); safety_set_allowed_area( MAV_FRAME_LOCAL_NED, // TODO: use enum from lib From 89b5659d4976329bdda2124e5f6d3f9f1ad6c693 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Thu, 9 Jul 2015 19:00:13 +0300 Subject: [PATCH 05/20] extras: mocap #319: use eigen based transform --- .../src/plugins/mocap_pose_estimate.cpp | 42 +++++++++---------- 1 file changed, 20 insertions(+), 22 deletions(-) diff --git a/mavros_extras/src/plugins/mocap_pose_estimate.cpp b/mavros_extras/src/plugins/mocap_pose_estimate.cpp index fbf4fcdf1..695d2290f 100644 --- a/mavros_extras/src/plugins/mocap_pose_estimate.cpp +++ b/mavros_extras/src/plugins/mocap_pose_estimate.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -92,19 +93,18 @@ class MocapPoseEstimatePlugin : public MavRosPlugin { float q[4]; - tf::Quaternion qo; - quaternionMsgToTF(pose->pose.orientation,qo); - auto qt = UAS::transform_frame_enu_ned_attitude_q(qo); + // Eigen has same order as mavlink: w x y z + Eigen::Quaterniond q_enu; + Eigen::Map q_out(q); - q[0] = qt.w(); - q[1] = qt.x(); - q[2] = qt.y(); - q[3] = qt.z(); + tf::quaternionMsgToEigen(pose->pose.orientation, q_enu); + q_out = UAS::transform_frame_enu_ned(q_enu).cast(); - auto position = UAS::transform_frame_enu_ned_xyz( + auto position = UAS::transform_frame_enu_ned( + Eigen::Vector3d( pose->pose.position.x, pose->pose.position.y, - pose->pose.position.z); + pose->pose.position.z)); mocap_pose_send(pose->header.stamp.toNSec() / 1000, q, @@ -117,21 +117,19 @@ class MocapPoseEstimatePlugin : public MavRosPlugin void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans) { float q[4]; - - tf::Transform tf; - transformMsgToTF(trans->transform,tf); - tf::Quaternion qo = tf.getRotation(); - auto qt = UAS::transform_frame_enu_ned_attitude_q(qo); - - q[0] = qt.w(); - q[1] = qt.x(); - q[2] = qt.y(); - q[3] = qt.z(); - - auto position = UAS::transform_frame_enu_ned_xyz( + + // Eigen has same order as mavlink: w x y z + Eigen::Quaterniond q_enu; + Eigen::Map q_out(q); + + tf::quaternionMsgToEigen(trans->transform.rotation, q_enu); + q_out = UAS::transform_frame_enu_ned(q_enu).cast(); + + auto position = UAS::transform_frame_enu_ned( + Eigen::Vector3d( trans->transform.translation.x, trans->transform.translation.y, - trans->transform.translation.z); + trans->transform.translation.z)); mocap_pose_send(trans->header.stamp.toNSec() / 1000, q, From 61b1875e03ac91eb69e6fee34b65981ebf09b991 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Thu, 9 Jul 2015 19:10:27 +0300 Subject: [PATCH 06/20] extras: px4flow #319: change transform_frame() --- mavros_extras/src/plugins/px4flow.cpp | 42 ++++++++++++--------------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/mavros_extras/src/plugins/px4flow.cpp b/mavros_extras/src/plugins/px4flow.cpp index e36bffeef..4c19d579e 100644 --- a/mavros_extras/src/plugins/px4flow.cpp +++ b/mavros_extras/src/plugins/px4flow.cpp @@ -78,9 +78,7 @@ class PX4FlowPlugin : public MavRosPlugin { mavlink_optical_flow_rad_t flow_rad; mavlink_msg_optical_flow_rad_decode(msg, &flow_rad); - std_msgs::Header header; - header.stamp = ros::Time::now(); - header.frame_id = frame_id; + auto header = uas->synchronized_header(frame_id, flow_rad.time_usec); /** * Raw message with axes mapped to ROS conventions and temp in degrees celsius. @@ -89,33 +87,32 @@ class PX4FlowPlugin : public MavRosPlugin { * gyroscope. (body-fixed NED -> ENU) */ - auto flow_rad_msg = boost::make_shared(); - - flow_rad_msg->header = header; - - flow_rad_msg->integration_time_us = flow_rad.integration_time_us; - auto position = UAS::transform_frame_enu_ned_xyz( + auto int_xy = UAS::transform_frame_enu_ned( + Eigen::Vector3d( flow_rad.integrated_x, flow_rad.integrated_y, - 0.0); - - flow_rad_msg->integrated_x = position.x(); - flow_rad_msg->integrated_y = position.y(); - - auto flow_gyro = UAS::transform_frame_enu_ned_xyz( + 0.0)); + auto int_gyro = UAS::transform_frame_enu_ned( + Eigen::Vector3d( flow_rad.integrated_xgyro, flow_rad.integrated_ygyro, - flow_rad.integrated_zgyro); + flow_rad.integrated_zgyro)); + + auto flow_rad_msg = boost::make_shared(); - flow_rad_msg->integrated_xgyro = flow_gyro.x(); - flow_rad_msg->integrated_ygyro = flow_gyro.y(); - flow_rad_msg->integrated_zgyro = flow_gyro.z(); + flow_rad_msg->header = header; + flow_rad_msg->integration_time_us = flow_rad.integration_time_us; - flow_rad_msg->temperature = flow_rad.temperature / 100.0f; // in degrees celsius + flow_rad_msg->integrated_x = int_xy.x(); + flow_rad_msg->integrated_y = int_xy.y(); - flow_rad_msg->time_delta_distance_us = flow_rad.time_delta_distance_us; + flow_rad_msg->integrated_xgyro = int_gyro.x(); + flow_rad_msg->integrated_ygyro = int_gyro.y(); + flow_rad_msg->integrated_zgyro = int_gyro.z(); + flow_rad_msg->temperature = flow_rad.temperature / 100.0f; // in degrees celsius + flow_rad_msg->time_delta_distance_us = flow_rad.time_delta_distance_us; flow_rad_msg->distance = flow_rad.distance; flow_rad_pub.publish(flow_rad_msg); @@ -124,8 +121,7 @@ class PX4FlowPlugin : public MavRosPlugin { auto temp_msg = boost::make_shared(); temp_msg->header = header; - - temp_msg->temperature = flow_rad.temperature / 100.0f; + temp_msg->temperature = flow_rad_msg->temperature; temp_pub.publish(temp_msg); From c18ae4667a8dd1bb55dd3890b1e1db8fd3e335c9 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Thu, 9 Jul 2015 19:14:41 +0300 Subject: [PATCH 07/20] extras: vibration: Use UAS::synchronized_header() --- mavros_extras/src/plugins/vibration.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/mavros_extras/src/plugins/vibration.cpp b/mavros_extras/src/plugins/vibration.cpp index b04de2ad9..708656b6e 100644 --- a/mavros_extras/src/plugins/vibration.cpp +++ b/mavros_extras/src/plugins/vibration.cpp @@ -59,14 +59,11 @@ class VibrationPlugin : public MavRosPlugin { mavlink_vibration_t vibration; mavlink_msg_vibration_decode(msg, &vibration); - std_msgs::Header header; - header.stamp = uas->synchronise_stamp(vibration.time_usec); - header.frame_id = frame_id; - auto vibe_msg = boost::make_shared(); - vibe_msg->header = header; + vibe_msg->header = uas->synchronized_header(frame_id, vibration.time_usec); + // TODO no transform_frame? vibe_msg->vibration.x = vibration.vibration_x; vibe_msg->vibration.y = vibration.vibration_y; vibe_msg->vibration.z = vibration.vibration_z; From 7e381abe0d59e76aeea970a2ee7fe317476db06d Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Thu, 9 Jul 2015 19:30:30 +0300 Subject: [PATCH 08/20] extras: vision_speed #319: use eigen based transform --- mavros_extras/src/plugins/vision_speed_estimate.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/mavros_extras/src/plugins/vision_speed_estimate.cpp b/mavros_extras/src/plugins/vision_speed_estimate.cpp index 09acd5a59..5af1b4ff1 100644 --- a/mavros_extras/src/plugins/vision_speed_estimate.cpp +++ b/mavros_extras/src/plugins/vision_speed_estimate.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -80,8 +81,10 @@ class VisionSpeedEstimatePlugin : public MavRosPlugin { /** * @brief Send vision speed estimate to FCU velocity controller */ - void send_vision_speed(float vx, float vy, float vz, const ros::Time &stamp) { - auto vel = UAS::transform_frame_enu_ned_xyz(vx, vy, vz); + void send_vision_speed(const geometry_msgs::Vector3 &vel_enu, const ros::Time &stamp) { + Eigen::Vector3d vel_; + tf::vectorMsgToEigen(vel_enu, vel_); + auto vel = UAS::transform_frame_enu_ned(vel_); vision_speed_estimate(stamp.toNSec() / 1000, vel.x(), vel.y(), vel.z()); @@ -90,11 +93,11 @@ class VisionSpeedEstimatePlugin : public MavRosPlugin { /* -*- callbacks -*- */ void vel_twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) { - send_vision_speed(req->twist.linear.x, req->twist.linear.y, req->twist.linear.z, req->header.stamp); + send_vision_speed(req->twist.linear, req->header.stamp); } void vel_speed_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) { - send_vision_speed(req->vector.x, req->vector.y, req->vector.z, req->header.stamp); + send_vision_speed(req->vector, req->header.stamp); } }; }; // namespace mavplugin From dc6f5e314228e5734b9dc8750e248c0f46a74897 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 16:18:11 +0300 Subject: [PATCH 09/20] lib: Add UAS::quaternion_to_rpy() --- mavros/include/mavros/mavros_uas.h | 27 +++++++++++++++++++----- mavros/src/lib/uas_frame_conversions.cpp | 25 ++++++++++++++++------ mavros/test/test_frame_conv.cpp | 10 +++++++++ 3 files changed, 50 insertions(+), 12 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h index 089713aae..c7435d28a 100644 --- a/mavros/include/mavros/mavros_uas.h +++ b/mavros/include/mavros/mavros_uas.h @@ -330,16 +330,33 @@ class UAS { /** * @brief Convert euler angles to quaternion. - * - * @return quaternion, same as @p tf::quaternionFromRPY() but in Eigen format. */ - static Eigen::Quaterniond quaternion_from_rpy(const double roll, const double pitch, const double yaw); + static Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy); /** * @brief Convert euler angles to quaternion. + * + * @return quaternion, same as @a tf::quaternionFromRPY() but in Eigen format. + */ + static inline Eigen::Quaterniond quaternion_from_rpy(const double roll, const double pitch, const double yaw) { + return quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw)); + } + + /** + * @brief Convert quaternion to euler angles + * + * Reverse operation to @a quaternion_from_rpy() + */ + static Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q); + + /** + * @brief Convert quaternion to euler angles */ - static inline Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &vec) { - return quaternion_from_rpy(vec.x(), vec.y(), vec.z()); + static inline void quaternion_to_rpy(const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw) { + const auto rpy = quaternion_to_rpy(q); + roll = rpy.x(); + pitch = rpy.y(); + yaw = rpy.z(); } /** diff --git a/mavros/src/lib/uas_frame_conversions.cpp b/mavros/src/lib/uas_frame_conversions.cpp index d4804b012..81495f588 100644 --- a/mavros/src/lib/uas_frame_conversions.cpp +++ b/mavros/src/lib/uas_frame_conversions.cpp @@ -127,25 +127,36 @@ static const Eigen::Quaterniond FRAME_ROTATE_Q = UAS::quaternion_from_rpy(M_PI, static const Eigen::Transform FRAME_TRANSFORM_VECTOR3(FRAME_ROTATE_Q); -Eigen::Quaterniond UAS::quaternion_from_rpy(const double roll, const double pitch, const double yaw) +Eigen::Quaterniond UAS::quaternion_from_rpy(const Eigen::Vector3d &rpy) { #if 0 // RPY - XYZ return Eigen::Quaterniond( - Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) ); #else // YPR - ZYX return Eigen::Quaterniond( - Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) + Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()) ); #endif } +Eigen::Vector3d UAS::quaternion_to_rpy(const Eigen::Quaterniond &q) +{ +#if 0 + // RPY - XYZ + return q.toRotationMatrix().eulerAngles(0, 1, 2); +#else + // YPR - ZYX + return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse(); +#endif +} + Eigen::Quaterniond UAS::transform_frame(const Eigen::Quaterniond &q) { return FRAME_ROTATE_Q * q * FRAME_ROTATE_Q.inverse(); diff --git a/mavros/test/test_frame_conv.cpp b/mavros/test/test_frame_conv.cpp index 88829388a..ae593a40e 100644 --- a/mavros/test/test_frame_conv.cpp +++ b/mavros/test/test_frame_conv.cpp @@ -129,6 +129,16 @@ TEST(EIGEN, quaternion_from_rpy__paranoic_check) EXPECT_NEAR(q1.z(), q2.z(), epsilon); } +TEST(EIGEN, quaternion_to_rpy__123) +{ + auto q = UAS::quaternion_from_rpy(1.0, 2.0, 3.0); + auto rpy = UAS::quaternion_to_rpy(q); + + EXPECT_NEAR(1.0, rpy.x(), epsilon); + EXPECT_NEAR(2.0, rpy.y(), epsilon); + EXPECT_NEAR(3.0, rpy.z(), epsilon); +} + // XXX: #321 comment out broken transform's before release 0.12 // after we SHOULD come and fix! #if 0 From 61cc2fe57cf80af3459464fd395964f467fbc5fe Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 16:57:14 +0300 Subject: [PATCH 10/20] extras: viz #336: convert plugin to node. --- mavros_extras/CMakeLists.txt | 10 +- mavros_extras/mavros_plugins.xml | 3 - mavros_extras/src/copter_visualization.cpp | 177 +++++++++++++++++ mavros_extras/src/plugins/visualization.cpp | 205 -------------------- 4 files changed, 185 insertions(+), 210 deletions(-) create mode 100644 mavros_extras/src/copter_visualization.cpp delete mode 100644 mavros_extras/src/plugins/visualization.cpp diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index 7261eb5e9..eab20477a 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -114,7 +114,6 @@ add_library(mavros_extras src/plugins/mocap_pose_estimate.cpp src/plugins/vision_pose_estimate.cpp src/plugins/vision_speed_estimate.cpp - src/plugins/visualization.cpp src/plugins/vibration.cpp ) add_dependencies(mavros_extras @@ -134,6 +133,13 @@ target_link_libraries(gcs_image_bridge ${catkin_LIBRARIES} ) +add_executable(copter_visualization + src/copter_visualization.cpp +) +target_link_libraries(copter_visualization + ${catkin_LIBRARIES} +) + ############# ## Install ## ############# @@ -150,7 +156,7 @@ install(PROGRAMS ) ## Mark executables and/or libraries for installation -install(TARGETS mavros_extras gcs_image_bridge +install(TARGETS mavros_extras gcs_image_bridge copter_visualization ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/mavros_extras/mavros_plugins.xml b/mavros_extras/mavros_plugins.xml index 81a3c20ab..d30a6ea51 100644 --- a/mavros_extras/mavros_plugins.xml +++ b/mavros_extras/mavros_plugins.xml @@ -17,9 +17,6 @@ Send vision speed estimate to FCU. - - Display position track of MAV in RViz. - Publish VIBRATION message data from FCU. diff --git a/mavros_extras/src/copter_visualization.cpp b/mavros_extras/src/copter_visualization.cpp new file mode 100644 index 000000000..1811f1c55 --- /dev/null +++ b/mavros_extras/src/copter_visualization.cpp @@ -0,0 +1,177 @@ +/** + * @brief Copter visualization + * @file copter_visualization.cpp + * @author M.H.Kabir + */ +/* + * Copyright 2014,2015 M.H.Kabir + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ + +#include +#include + +#include +#include + +// parameters +static std::string fixed_frame_id; +static std::string child_frame_id; +static double marker_scale; + +// merker publishers +ros::Publisher track_marker_pub; +ros::Publisher vehicle_marker_pub; + + +/** + * @brief publish vehicle track + */ +static void publish_track_marker(const geometry_msgs::PoseStamped::ConstPtr &pose) +{ + static int marker_id = 0; + + auto marker = boost::make_shared(); + + marker->header = pose->header; + marker->type = visualization_msgs::Marker::CUBE; + marker->ns = "fcu"; + marker->id = marker_id++; + marker->action = visualization_msgs::Marker::ADD; + marker->pose = pose->pose; + marker->scale.x = marker_scale * 0.015; + marker->scale.y = marker_scale * 0.015; + marker->scale.z = marker_scale * 0.015; + + marker->color.a = 1.0; + marker->color.r = 0.0; + marker->color.g = 0.0; + marker->color.b = 0.5; + + track_marker_pub.publish(marker); +} + +/** + * @brief publish vehicle + */ +static void publish_vehicle_marker() { + /** Hexacopter marker code adapted from libsfly_viz + * thanks to Markus Achtelik. + */ + + const double sqrt2_2 = sqrt(2) / 2; + int id = 1; + + auto marker = boost::make_shared(); + + // the marker will be displayed in frame_id + marker->header.stamp = ros::Time(); + marker->header.frame_id = child_frame_id; + marker->ns = "fcu"; + marker->action = visualization_msgs::Marker::ADD; + marker->id = id; + + // make rotors + marker->type = visualization_msgs::Marker::CYLINDER; + marker->scale.x = 0.2 * marker_scale; + marker->scale.y = 0.2 * marker_scale; + marker->scale.z = 0.01 * marker_scale; + marker->color.r = 0.4; + marker->color.g = 0.4; + marker->color.b = 0.4; + marker->color.a = 0.8; + marker->pose.position.z = 0; + + // front left/right + marker->pose.position.x = 0.19 * marker_scale; + marker->pose.position.y = 0.11 * marker_scale; + marker->id++; // in older code there be decrement + vehicle_marker_pub.publish(marker); + + marker->pose.position.x = 0.19 * marker_scale; + marker->pose.position.y = -0.11 * marker_scale; + marker->id++; + vehicle_marker_pub.publish(marker); + + // left/right + marker->pose.position.x = 0; + marker->pose.position.y = 0.22 * marker_scale; + marker->id++; + vehicle_marker_pub.publish(marker); + + marker->pose.position.x = 0; + marker->pose.position.y = -0.22 * marker_scale; + marker->id++; + vehicle_marker_pub.publish(marker); + + // back left/right + marker->pose.position.x = -0.19 * marker_scale; + marker->pose.position.y = 0.11 * marker_scale; + marker->id++; + vehicle_marker_pub.publish(marker); + + marker->pose.position.x = -0.19 * marker_scale; + marker->pose.position.y = -0.11 * marker_scale; + marker->id++; + vehicle_marker_pub.publish(marker); + + // make arms + marker->type = visualization_msgs::Marker::CUBE; + marker->scale.x = 0.44 * marker_scale; + marker->scale.y = 0.02 * marker_scale; + marker->scale.z = 0.01 * marker_scale; + marker->color.r = 0.0; + marker->color.g = 0.0; + marker->color.b = 1.0; + marker->color.a = 1.0; + + marker->pose.position.x = 0; + marker->pose.position.y = 0; + marker->pose.position.z = -0.015 * marker_scale; + marker->pose.orientation.x = 0; + marker->pose.orientation.y = 0; + + marker->pose.orientation.w = sqrt2_2; + marker->pose.orientation.z = sqrt2_2; + marker->id++; + vehicle_marker_pub.publish(marker); + + // 30 deg rotation 0.9659 0 0 0.2588 + marker->pose.orientation.w = 0.9659; + marker->pose.orientation.z = 0.2588; + marker->id++; + vehicle_marker_pub.publish(marker); + + marker->pose.orientation.w = 0.9659; + marker->pose.orientation.z = -0.2588; + marker->id++; + vehicle_marker_pub.publish(marker); +} + +static void local_position_sub_cb(const geometry_msgs::PoseStamped::ConstPtr &pose) +{ + publish_track_marker(pose); + publish_vehicle_marker(); +} + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "copter_visualization"); + ros::NodeHandle nh; + ros::NodeHandle priv_nh("~"); + + priv_nh.param("fixed_frame_id", fixed_frame_id, "local_origin"); + priv_nh.param("child_frame_id", child_frame_id, "fcu"); + priv_nh.param("marker_scale", marker_scale, 2.0); + + track_marker_pub = nh.advertise("track_markers", 10); + vehicle_marker_pub = nh.advertise("vehicle_marker", 10); + + auto sub = nh.subscribe("local_position", 10, local_position_sub_cb); + + ros::spin(); + return 0; +} diff --git a/mavros_extras/src/plugins/visualization.cpp b/mavros_extras/src/plugins/visualization.cpp deleted file mode 100644 index e8e066db7..000000000 --- a/mavros_extras/src/plugins/visualization.cpp +++ /dev/null @@ -1,205 +0,0 @@ -/** - * @brief visualization plugin - * @file visualization.cpp - * @author M.H.Kabir - * - * @addtogroup plugin - * @{ - */ -/* - * Copyright 2014 M.H.Kabir - * - * This file is part of the mavros package and subject to the license terms - * in the top-level LICENSE file of the mavros repository. - * https://github.com/mavlink/mavros/tree/master/LICENSE.md - */ - -#include -#include - -#include - -namespace mavplugin { -/** - * @brief RViz Visualiser plugin. - * Display track of MAV's path in RViz and current position target. - */ -class VisualizationPlugin : public MavRosPlugin { -public: - VisualizationPlugin() : - viz_nh("~visualization"), - uas(nullptr), - marker_scale(0), - marker_id(0) - { }; - - void initialize(UAS &uas_) - { - uas = &uas_; - - viz_nh.param("fixed_frame_id", fixed_frame_id, "local_origin"); - viz_nh.param("child_frame_id", child_frame_id, "fcu"); - viz_nh.param("marker_scale", marker_scale, 2.0); - - track_marker = viz_nh.advertise("track_markers", 10); - vehicle_marker = viz_nh.advertise("vehicle_marker", 10); - } - - const message_map get_rx_handlers() { - return { - MESSAGE_HANDLER(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &VisualizationPlugin::handle_local_position_ned) - // XXX Add handling of position target. - }; - } - -private: - ros::NodeHandle viz_nh; - UAS *uas; - - ros::Publisher track_marker; - ros::Publisher vehicle_marker; - - std::string fixed_frame_id; - std::string child_frame_id; // frame for visualization markers - - double marker_scale; - int marker_id; - - void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { - mavlink_local_position_ned_t pos_ned; - mavlink_msg_local_position_ned_decode(msg, &pos_ned); - - tf::Transform transform; - transform.setOrigin(tf::Vector3(pos_ned.x, -pos_ned.y, -pos_ned.z)); - transform.setRotation(uas->get_attitude_orientation()); - - auto pose = boost::make_shared(); - - tf::poseTFToMsg(transform, pose->pose); - pose->header.frame_id = fixed_frame_id; - pose->header.stamp = ros::Time(); - - publish_vehicle_marker(); - publish_vis_marker(pose); - } - - void publish_vis_marker(geometry_msgs::PoseStampedPtr pose) - { - auto marker = boost::make_shared(); - - marker->header = pose->header; - marker->type = visualization_msgs::Marker::CUBE; - marker->ns = "fcu"; - marker->id = marker_id++; - marker->action = visualization_msgs::Marker::ADD; - marker->pose = pose->pose; - marker->scale.x = marker_scale * 0.015; - marker->scale.y = marker_scale * 0.015; - marker->scale.z = marker_scale * 0.015; - - marker->color.a = 1.0; - marker->color.r = 0.0; - marker->color.g = 0.0; - marker->color.b = 0.5; - - track_marker.publish(marker); - } - - void publish_vehicle_marker() { - /** Hexacopter marker code adapted from libsfly_viz - * thanks to Markus Achtelik. - */ - - const double sqrt2_2 = sqrt(2) / 2; - int id = 1; - - auto marker = boost::make_shared(); - - // the marker will be displayed in frame_id - marker->header.stamp = ros::Time(); - marker->header.frame_id = child_frame_id; - marker->ns = "fcu"; - marker->action = visualization_msgs::Marker::ADD; - marker->id = id; - - // make rotors - marker->type = visualization_msgs::Marker::CYLINDER; - marker->scale.x = 0.2 * marker_scale; - marker->scale.y = 0.2 * marker_scale; - marker->scale.z = 0.01 * marker_scale; - marker->color.r = 0.4; - marker->color.g = 0.4; - marker->color.b = 0.4; - marker->color.a = 0.8; - marker->pose.position.z = 0; - - // front left/right - marker->pose.position.x = 0.19 * marker_scale; - marker->pose.position.y = 0.11 * marker_scale; - marker->id--; - vehicle_marker.publish(marker); - - marker->pose.position.x = 0.19 * marker_scale; - marker->pose.position.y = -0.11 * marker_scale; - marker->id--; - vehicle_marker.publish(marker); - - // left/right - marker->pose.position.x = 0; - marker->pose.position.y = 0.22 * marker_scale; - marker->id--; - vehicle_marker.publish(marker); - - marker->pose.position.x = 0; - marker->pose.position.y = -0.22 * marker_scale; - marker->id--; - vehicle_marker.publish(marker); - - // back left/right - marker->pose.position.x = -0.19 * marker_scale; - marker->pose.position.y = 0.11 * marker_scale; - marker->id--; - vehicle_marker.publish(marker); - - marker->pose.position.x = -0.19 * marker_scale; - marker->pose.position.y = -0.11 * marker_scale; - marker->id--; - vehicle_marker.publish(marker); - - // make arms - marker->type = visualization_msgs::Marker::CUBE; - marker->scale.x = 0.44 * marker_scale; - marker->scale.y = 0.02 * marker_scale; - marker->scale.z = 0.01 * marker_scale; - marker->color.r = 0.0; - marker->color.g = 0.0; - marker->color.b = 1.0; - marker->color.a = 1.0; - - marker->pose.position.x = 0; - marker->pose.position.y = 0; - marker->pose.position.z = -0.015 * marker_scale; - marker->pose.orientation.x = 0; - marker->pose.orientation.y = 0; - - marker->pose.orientation.w = sqrt2_2; - marker->pose.orientation.z = sqrt2_2; - marker->id--; - vehicle_marker.publish(marker); - - // 30 deg rotation 0.9659 0 0 0.2588 - marker->pose.orientation.w = 0.9659; - marker->pose.orientation.z = 0.2588; - marker->id--; - vehicle_marker.publish(marker); - - marker->pose.orientation.w = 0.9659; - marker->pose.orientation.z = -0.2588; - marker->id--; - vehicle_marker.publish(marker); - } -}; -}; // namespace mavplugin - -PLUGINLIB_EXPORT_CLASS(mavplugin::VisualizationPlugin, mavplugin::MavRosPlugin) - From 99d24d5528b9044e3f44d9edf245939ab75ec60f Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 17:10:20 +0300 Subject: [PATCH 11/20] launch: Update configs. --- mavros/launch/apm_config.yaml | 8 +------- mavros/launch/apm_pluginlists.yaml | 1 - mavros/launch/px4_config.yaml | 8 +------- mavros_extras/launch/px4flow_config.yaml | 10 +++++----- 4 files changed, 7 insertions(+), 20 deletions(-) diff --git a/mavros/launch/apm_config.yaml b/mavros/launch/apm_config.yaml index cc3fcf8c8..f9ea02a78 100644 --- a/mavros/launch/apm_config.yaml +++ b/mavros/launch/apm_config.yaml @@ -138,14 +138,8 @@ vision_pose: vision_speed: listen_twist: false -# visualization -visualization: - fixed_frame_id: "local_origin" - child_frame_id: "fcu" - marker_scale: 2.0 - # vibration vibration: frame_id: "vibration" -# vim:ts=2 sw=2 et: +# vim:set ts=2 sw=2 et: diff --git a/mavros/launch/apm_pluginlists.yaml b/mavros/launch/apm_pluginlists.yaml index 887d6581e..60ce0de0f 100644 --- a/mavros/launch/apm_pluginlists.yaml +++ b/mavros/launch/apm_pluginlists.yaml @@ -9,7 +9,6 @@ plugin_blacklist: - 'mocap_*' - px4flow - 'vision_*' -- visualization - distance_sensor - vibration diff --git a/mavros/launch/px4_config.yaml b/mavros/launch/px4_config.yaml index fb24470d8..d17e678ce 100644 --- a/mavros/launch/px4_config.yaml +++ b/mavros/launch/px4_config.yaml @@ -163,14 +163,8 @@ vision_pose: vision_speed: listen_twist: false -# visualization -visualization: - fixed_frame_id: "local_origin" - child_frame_id: "fcu" - marker_scale: 2.0 - # vibration vibration: frame_id: "vibration" -# vim:ts=2 sw=2 et: +# vim:set ts=2 sw=2 et: diff --git a/mavros_extras/launch/px4flow_config.yaml b/mavros_extras/launch/px4flow_config.yaml index 6a7f66d6b..0e822c09f 100644 --- a/mavros_extras/launch/px4flow_config.yaml +++ b/mavros_extras/launch/px4flow_config.yaml @@ -7,14 +7,14 @@ startup_px4_usb_quirk: false # sys_status & sys_time connection options conn: - heartbeat: 5.0 # send hertbeat every n seconds - timeout: 10.0 # hertbeat timeout in seconds - timesync: 0.0 # TIMESYNC period in seconds (feature disabled if 0.0) - system_time: 2.0 # send system time to FCU every n seconds + heartbeat_rate: 1.0 # send hertbeat rate in Hertz + timeout: 10.0 # hertbeat timeout in seconds + timesync_rate: 0.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) + system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) # sys_status sys: - min_voltage: 10.0 # diagnostics min voltage + min_voltage: 4.5 # diagnostics min voltage disable_diag: true # disable all sys_status diagnostics, except heartbeat # --- mavros extras plugins (same order) --- From 774f054a00cd9f1aa8e9db49a1491b6c1fb9eb80 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 17:58:12 +0300 Subject: [PATCH 12/20] uas #71: Use single TF2 objects for broadcasting and subscription. --- mavros/include/mavros/mavros_uas.h | 8 ++++++++ mavros/src/lib/uas_data.cpp | 1 + mavros/src/plugins/global_position.cpp | 5 +---- mavros/src/plugins/local_position.cpp | 4 +--- 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h index c7435d28a..c0687b649 100644 --- a/mavros/include/mavros/mavros_uas.h +++ b/mavros/include/mavros/mavros_uas.h @@ -22,6 +22,8 @@ #include #include #include +#include +#include #include #include @@ -202,6 +204,12 @@ class UAS { sensor_msgs::NavSatFix::Ptr get_gps_fix(); + /* -*- transform -*- */ + + tf2_ros::Buffer tf2_buffer; + tf2_ros::TransformListener tf2_listener; + tf2_ros::TransformBroadcaster tf2_broadcaster; + /* -*- time sync -*- */ inline void set_time_offset(uint64_t offset_ns) { diff --git a/mavros/src/lib/uas_data.cpp b/mavros/src/lib/uas_data.cpp index c738e464c..89f4f0f4d 100644 --- a/mavros/src/lib/uas_data.cpp +++ b/mavros/src/lib/uas_data.cpp @@ -21,6 +21,7 @@ using namespace mavros; UAS::UAS() : + tf2_listener(tf2_buffer, true), type(MAV_TYPE_GENERIC), autopilot(MAV_AUTOPILOT_GENERIC), target_system(1), diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp index 05a1e42b7..5935aea85 100644 --- a/mavros/src/plugins/global_position.cpp +++ b/mavros/src/plugins/global_position.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -95,8 +94,6 @@ class GlobalPositionPlugin : public MavRosPlugin { ros::Publisher gp_hdg_pub; ros::Publisher gp_rel_alt_pub; - tf2_ros::TransformBroadcaster tf2_broadcaster; - std::string frame_id; //!< frame for topic headers std::string tf_frame_id; //!< origin for TF std::string tf_child_frame_id; //!< frame for TF and Pose @@ -282,7 +279,7 @@ class GlobalPositionPlugin : public MavRosPlugin { transform.transform.translation.y = pose_cov->pose.pose.position.y; transform.transform.translation.z = pose_cov->pose.pose.position.z; - tf2_broadcaster.sendTransform(transform); + uas->tf2_broadcaster.sendTransform(transform); } } diff --git a/mavros/src/plugins/local_position.cpp b/mavros/src/plugins/local_position.cpp index 5c2ba9336..a04c7903c 100644 --- a/mavros/src/plugins/local_position.cpp +++ b/mavros/src/plugins/local_position.cpp @@ -17,7 +17,6 @@ #include #include -#include #include #include @@ -61,7 +60,6 @@ class LocalPositionPlugin : public MavRosPlugin { UAS *uas; ros::Publisher local_position; - tf2_ros::TransformBroadcaster tf2_broadcaster; std::string frame_id; //!< frame for Pose std::string tf_frame_id; //!< origin for TF @@ -96,7 +94,7 @@ class LocalPositionPlugin : public MavRosPlugin { transform.transform.rotation = pose->pose.orientation; tf::vectorEigenToMsg(position, transform.transform.translation); - tf2_broadcaster.sendTransform(transform); + uas->tf2_broadcaster.sendTransform(transform); } } }; From 411b7e7e1af0bd39273633e0c6f87bea6dcb8a54 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 21:23:03 +0300 Subject: [PATCH 13/20] plugin #71: Implement TF2 listener. Change param names. Breaks extras. --- mavros/include/mavros/mavros_uas.h | 9 ++ mavros/include/mavros/setpoint_mixin.h | 51 +++++------ mavros/launch/apm_config.yaml | 20 ++--- mavros/launch/px4_config.yaml | 20 ++--- mavros/src/plugins/setpoint_attitude.cpp | 105 ++++++++++------------- mavros/src/plugins/setpoint_position.cpp | 60 +++++++------ 6 files changed, 132 insertions(+), 133 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h index c0687b649..70246dd05 100644 --- a/mavros/include/mavros/mavros_uas.h +++ b/mavros/include/mavros/mavros_uas.h @@ -367,6 +367,15 @@ class UAS { yaw = rpy.z(); } + /** + * @brief Get Yaw angle from quaternion + * + * Replacement function for @a tf::getYaw() + */ + static inline double getYaw(const Eigen::Quaterniond &q) { + return quaternion_to_rpy(q).z(); + } + /** * @brief Transform frame between ROS and FCU. (Vector3d) * diff --git a/mavros/include/mavros/setpoint_mixin.h b/mavros/include/mavros/setpoint_mixin.h index 7c5b17090..da8fca94b 100644 --- a/mavros/include/mavros/setpoint_mixin.h +++ b/mavros/include/mavros/setpoint_mixin.h @@ -19,7 +19,7 @@ #include #include -#include +#include namespace mavplugin { /** @@ -52,17 +52,18 @@ class SetPositionTargetLocalNEDMixin { }; /** - * @brief This mixin adds TF listener to plugin + * @brief This mixin adds TF2 listener thread to plugin * - * It requires ros::NodeHandle named sp_nh, - * frame_id and child_frame_id strings and tf_rate double. + * It requires tf_frame_id, tf_child_frame_id strings + * tf_rate double and uas object pointer */ template -class TFListenerMixin { +class TF2ListenerMixin { public: + ros::NodeHandle tf_nh; std::thread tf_thread; - std::string thd_name; - boost::function tf_transform_cb; + std::string tf_thd_name; + boost::function tf_transform_cb; /** * @brief start tf listener @@ -70,32 +71,32 @@ class TFListenerMixin { * @param _thd_name listener thread name * @param cbp plugin callback function */ - void tf_start(const char *_thd_name, void (D::*cbp)(const tf::Transform &, const ros::Time &) ) { - thd_name = _thd_name; - tf_transform_cb = boost::bind(cbp, static_cast(this), _1, _2); + void tf2_start(const char *_thd_name, void (D::*cbp)(const geometry_msgs::TransformStamped &) ) { + tf_thd_name = _thd_name; + tf_transform_cb = boost::bind(cbp, static_cast(this), _1); - std::thread t(boost::bind(&TFListenerMixin::tf_listener, this)); - mavutils::set_thread_name(t, thd_name); + std::thread t(boost::bind(&TF2ListenerMixin::tf_listener, this)); + mavutils::set_thread_name(t, tf_thd_name); tf_thread.swap(t); } void tf_listener(void) { - ros::NodeHandle &_sp_nh = static_cast(this)->sp_nh; - std::string &_frame_id = static_cast(this)->frame_id; - std::string &_child_frame_id = static_cast(this)->child_frame_id; + mavros::UAS *_uas = static_cast(this)->uas; + std::string &_frame_id = static_cast(this)->tf_frame_id; + std::string &_child_frame_id = static_cast(this)->tf_child_frame_id; - tf::TransformListener listener(_sp_nh); - tf::StampedTransform transform; ros::Rate rate(static_cast(this)->tf_rate); - while (_sp_nh.ok()) { + while (tf_nh.ok()) { // Wait up to 3s for transform - listener.waitForTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0)); - try { - listener.lookupTransform(_frame_id, _child_frame_id, ros::Time(0), transform); - tf_transform_cb(static_cast(transform), transform.stamp_); - } - catch (tf::TransformException ex) { - ROS_ERROR_NAMED("setpoint", "%s: %s", thd_name.c_str(), ex.what()); + if (_uas->tf2_buffer.canTransform(_frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0))) { + try { + auto transform = _uas->tf2_buffer.lookupTransform( + _frame_id, _child_frame_id, ros::Time(0), ros::Duration(3.0)); + tf_transform_cb(transform); + } + catch (tf2::LookupException &ex) { + ROS_ERROR_NAMED("tf2_buffer", "%s: %s", tf_thd_name.c_str(), ex.what()); + } } rate.sleep(); } diff --git a/mavros/launch/apm_config.yaml b/mavros/launch/apm_config.yaml index f9ea02a78..1046401f9 100644 --- a/mavros/launch/apm_config.yaml +++ b/mavros/launch/apm_config.yaml @@ -82,20 +82,20 @@ setpoint_accel: # setpoint_attitude setpoint_attitude: - listen_tf: false - listen_twist: false - pose_with_covariance: false - frame_id: "local_origin" - child_frame_id: "attitude" - tf_rate_limit: 10.0 reverse_throttle: false # allow reversed throttle + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "local_origin" + child_frame_id: "attitude" + rate_limit: 10.0 # setpoint_position setpoint_position: - listen_tf: false - frame_id: "local_origin" - child_frame_id: "setpoint" - tf_rate_limit: 50.0 + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "local_origin" + child_frame_id: "setpoint" + rate_limit: 50.0 # setpoint_velocity # None diff --git a/mavros/launch/px4_config.yaml b/mavros/launch/px4_config.yaml index d17e678ce..3fac35f83 100644 --- a/mavros/launch/px4_config.yaml +++ b/mavros/launch/px4_config.yaml @@ -82,20 +82,20 @@ setpoint_accel: # setpoint_attitude setpoint_attitude: - listen_tf: false - listen_twist: false - pose_with_covariance: false - frame_id: "local_origin" - child_frame_id: "attitude" - tf_rate_limit: 10.0 reverse_throttle: false # allow reversed throttle + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "local_origin" + child_frame_id: "attitude" + rate_limit: 10.0 # setpoint_position setpoint_position: - listen_tf: false - frame_id: "local_origin" - child_frame_id: "setpoint" - tf_rate_limit: 50.0 + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "local_origin" + child_frame_id: "setpoint" + rate_limit: 50.0 # setpoint_velocity # None diff --git a/mavros/src/plugins/setpoint_attitude.cpp b/mavros/src/plugins/setpoint_attitude.cpp index 232e35757..24df7d40f 100644 --- a/mavros/src/plugins/setpoint_attitude.cpp +++ b/mavros/src/plugins/setpoint_attitude.cpp @@ -18,9 +18,9 @@ #include #include #include +#include #include -#include #include #include @@ -31,7 +31,7 @@ namespace mavplugin { * Send setpoint attitude/orientation/thrust to FCU controller. */ class SetpointAttitudePlugin : public MavRosPlugin, - private TFListenerMixin { + private TF2ListenerMixin { public: SetpointAttitudePlugin() : sp_nh("~setpoint_attitude"), @@ -42,37 +42,26 @@ class SetpointAttitudePlugin : public MavRosPlugin, void initialize(UAS &uas_) { - bool pose_with_covariance; - bool listen_tf; - bool listen_twist; + bool tf_listen; uas = &uas_; - sp_nh.param("listen_twist", listen_twist, true); - sp_nh.param("pose_with_covariance", pose_with_covariance, false); - // may be used to mimic attitude of an object, a gesture, etc. - sp_nh.param("listen_tf", listen_tf, false); - sp_nh.param("frame_id", frame_id, "local_origin"); - sp_nh.param("child_frame_id", child_frame_id, "attitude"); - sp_nh.param("tf_rate_limit", tf_rate, 10.0); + // main params sp_nh.param("reverse_throttle", reverse_throttle, false); - - if (listen_tf) { - ROS_INFO_STREAM_NAMED("attitude", "Listen to desired attitude transform " << frame_id - << " -> " << child_frame_id); - tf_start("AttitudeSpTF", &SetpointAttitudePlugin::send_attitude_transform); - } - else if (listen_twist) { - ROS_DEBUG_NAMED("attitude", "Setpoint attitude topic type: Twist"); - att_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this); - } - else if (pose_with_covariance) { - ROS_DEBUG_NAMED("attitude", "Setpoint attitude topic type: PoseWithCovarianceStamped"); - att_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cov_cb, this); + // tf params + sp_nh.param("tf/listen", tf_listen, false); + sp_nh.param("tf/frame_id", tf_frame_id, "local_origin"); + sp_nh.param("tf/child_frame_id", tf_child_frame_id, "attitude"); + sp_nh.param("tf/rate_limit", tf_rate, 10.0); + + if (tf_listen) { + ROS_INFO_STREAM_NAMED("attitude", "Listen to desired attitude transform " << tf_frame_id + << " -> " << tf_child_frame_id); + tf2_start("AttitudeSpTF", &SetpointAttitudePlugin::transform_cb); } else { - ROS_DEBUG_NAMED("attitude", "Setpoint attitude topic type: PoseStamped"); - att_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this); + twist_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this); + pose_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this); } throttle_sub = sp_nh.subscribe("att_throttle", 10, &SetpointAttitudePlugin::throttle_cb, this); @@ -83,16 +72,16 @@ class SetpointAttitudePlugin : public MavRosPlugin, } private: - friend class TFListenerMixin; + friend class TF2ListenerMixin; ros::NodeHandle sp_nh; UAS *uas; - ros::Subscriber att_sub; + ros::Subscriber twist_sub; + ros::Subscriber pose_sub; ros::Subscriber throttle_sub; - std::string frame_id; - std::string child_frame_id; - + std::string tf_frame_id; + std::string tf_child_frame_id; double tf_rate; bool reverse_throttle; @@ -121,21 +110,15 @@ class SetpointAttitudePlugin : public MavRosPlugin, * * @note ENU frame. */ - void send_attitude_transform(const tf::Transform &transform, const ros::Time &stamp) { - /** - * Thrust + RPY, also bits numbering started from 1 in docs - */ + void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr) { + /* Thrust + RPY, also bits numbering started from 1 in docs + */ const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0); float q[4]; - tf::Quaternion tf_q = transform.getRotation(); - - auto qt = UAS::transform_frame_enu_ned_attitude_q(tf_q); - - q[0] = qt.w(); - q[1] = qt.x(); - q[2] = qt.y(); - q[3] = qt.z(); + // Eigen use same convention as mavlink: w x y z + Eigen::Map q_out(q); + q_out = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation())).cast(); set_attitude_target(stamp.toNSec() / 1000000, ignore_all_except_q, @@ -149,19 +132,18 @@ class SetpointAttitudePlugin : public MavRosPlugin, * * @note ENU frame. */ - void send_attitude_ang_velocity(const ros::Time &stamp, const double vx, const double vy, const double vz) { - /** - * Q + Thrust, also bits noumbering started from 1 in docs + void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel) { + /* Q + Thrust, also bits noumbering started from 1 in docs */ const uint8_t ignore_all_except_rpy = (1 << 7) | (1 << 6); float q[4] = { 1.0, 0.0, 0.0, 0.0 }; - auto vel = UAS::transform_frame_enu_ned_xyz(vx, vy, vz); + auto av = UAS::transform_frame_enu_ned(ang_vel); set_attitude_target(stamp.toNSec() / 1000000, ignore_all_except_rpy, q, - vel.x(), vel.y(), vel.z(), + av.x(), av.y(), av.z(), 0.0); } @@ -182,24 +164,25 @@ class SetpointAttitudePlugin : public MavRosPlugin, /* -*- callbacks -*- */ - void pose_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req) { - tf::Transform transform; - poseMsgToTF(req->pose.pose, transform); - send_attitude_transform(transform, req->header.stamp); + void transform_cb(const geometry_msgs::TransformStamped &transform) { + Eigen::Affine3d tr; + tf::transformMsgToEigen(transform.transform, tr); + + send_attitude_target(transform.header.stamp, tr); } void pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req) { - tf::Transform transform; - poseMsgToTF(req->pose, transform); - send_attitude_transform(transform, req->header.stamp); + Eigen::Affine3d tr; + tf::poseMsgToEigen(req->pose, tr); + + send_attitude_target(req->header.stamp, tr); } void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) { - send_attitude_ang_velocity( - req->header.stamp, - req->twist.angular.x, - req->twist.angular.y, - req->twist.angular.z); + Eigen::Vector3d ang_vel; + tf::vectorMsgToEigen(req->twist.angular, ang_vel); + + send_attitude_ang_velocity(req->header.stamp, ang_vel); } inline bool is_normalized(float throttle, const float min, const float max) { diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp index 1405da7fb..1454b925d 100644 --- a/mavros/src/plugins/setpoint_position.cpp +++ b/mavros/src/plugins/setpoint_position.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include @@ -28,7 +29,7 @@ namespace mavplugin { */ class SetpointPositionPlugin : public MavRosPlugin, private SetPositionTargetLocalNEDMixin, - private TFListenerMixin { + private TF2ListenerMixin { public: SetpointPositionPlugin() : sp_nh("~setpoint_position"), @@ -38,19 +39,20 @@ class SetpointPositionPlugin : public MavRosPlugin, void initialize(UAS &uas_) { - bool listen_tf; + bool tf_listen; uas = &uas_; - sp_nh.param("listen_tf", listen_tf, false); - sp_nh.param("frame_id", frame_id, "local_origin"); - sp_nh.param("child_frame_id", child_frame_id, "setpoint"); - sp_nh.param("tf_rate_limit", tf_rate, 50.0); + // tf params + sp_nh.param("tf/listen", tf_listen, false); + sp_nh.param("tf/frame_id", tf_frame_id, "local_origin"); + sp_nh.param("tf/child_frame_id", tf_child_frame_id, "setpoint"); + sp_nh.param("tf/rate_limit", tf_rate, 50.0); - if (listen_tf) { - ROS_INFO_STREAM_NAMED("setpoint", "Listen to position setpoint transform " << frame_id - << " -> " << child_frame_id); - tf_start("PositionSpTF", &SetpointPositionPlugin::send_setpoint_transform); + if (tf_listen) { + ROS_INFO_STREAM_NAMED("setpoint", "Listen to position setpoint transform " << tf_frame_id + << " -> " << tf_child_frame_id); + tf2_start("PositionSpTF", &SetpointPositionPlugin::transform_cb); } else { setpoint_sub = sp_nh.subscribe("local", 10, &SetpointPositionPlugin::setpoint_cb, this); @@ -63,29 +65,24 @@ class SetpointPositionPlugin : public MavRosPlugin, private: friend class SetPositionTargetLocalNEDMixin; - friend class TFListenerMixin; + friend class TF2ListenerMixin; ros::NodeHandle sp_nh; UAS *uas; ros::Subscriber setpoint_sub; - std::string frame_id; - std::string child_frame_id; - + std::string tf_frame_id; + std::string tf_child_frame_id; double tf_rate; /* -*- mid-level helpers -*- */ /** - * @brief Send transform to FCU position controller. + * @brief Send setpoint to FCU position controller. * * @warning Send only XYZ, Yaw. ENU frame. */ - void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) { - // ENU frame - tf::Vector3 origin = transform.getOrigin(); - tf::Quaternion q = transform.getRotation(); - + void send_position_target(const ros::Time &stamp, const Eigen::Affine3d &tr) { /* Documentation start from bit 1 instead 0; * Ignore velocity and accel vectors, yaw rate. * @@ -94,26 +91,35 @@ class SetpointPositionPlugin : public MavRosPlugin, */ const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3); - auto position = UAS::transform_frame_enu_ned_xyz(origin.x(), origin.y(), origin.z()); - auto qt = UAS::transform_frame_enu_ned_attitude_q(q); + auto p = UAS::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); + auto q = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation())); set_position_target_local_ned(stamp.toNSec() / 1000000, MAV_FRAME_LOCAL_NED, ignore_all_except_xyz_y, - position.x(), position.y(), position.z(), + p.x(), p.y(), p.z(), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - tf::getYaw(qt), 0.0); + UAS::getYaw(q), 0.0); } /* -*- callbacks -*- */ /* common TF listener moved to mixin */ + void transform_cb(const geometry_msgs::TransformStamped &transform) { + Eigen::Affine3d tr; + // TODO: later, when tf2 5.12 will be released need to revisit and replace this to + // tf2::convert() + tf::transformMsgToEigen(transform.transform, tr); + + send_position_target(transform.header.stamp, tr); + } void setpoint_cb(const geometry_msgs::PoseStamped::ConstPtr &req) { - tf::Transform transform; - poseMsgToTF(req->pose, transform); - send_setpoint_transform(transform, req->header.stamp); + Eigen::Affine3d tr; + tf::poseMsgToEigen(req->pose, tr); + + send_position_target(req->header.stamp, tr); } }; }; // namespace mavplugin From f22ab3bfc9467965e9e9113a3ecc868e47d93fce Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 21:42:57 +0300 Subject: [PATCH 14/20] extras: vision_pose #71: Use TF2 listener. Also #319. --- mavros/launch/apm_config.yaml | 10 +-- mavros/launch/px4_config.yaml | 10 +-- .../src/plugins/vision_pose_estimate.cpp | 69 +++++++++---------- 3 files changed, 41 insertions(+), 48 deletions(-) diff --git a/mavros/launch/apm_config.yaml b/mavros/launch/apm_config.yaml index 1046401f9..cd1671c1d 100644 --- a/mavros/launch/apm_config.yaml +++ b/mavros/launch/apm_config.yaml @@ -128,11 +128,11 @@ px4flow: # vision_pose_estimate vision_pose: - pose_with_covariance: false - listen_tf: false - frame_id: "local_origin" - child_frame_id: "vision" - tf_rate_limit: 10.0 + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "local_origin" + child_frame_id: "vision" + rate_limit: 10.0 # vision_speed_estimate vision_speed: diff --git a/mavros/launch/px4_config.yaml b/mavros/launch/px4_config.yaml index 3fac35f83..0c7f1adce 100644 --- a/mavros/launch/px4_config.yaml +++ b/mavros/launch/px4_config.yaml @@ -153,11 +153,11 @@ px4flow: # vision_pose_estimate vision_pose: - pose_with_covariance: false - listen_tf: false - frame_id: "local_origin" - child_frame_id: "vision" - tf_rate_limit: 10.0 + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "local_origin" + child_frame_id: "vision" + rate_limit: 10.0 # vision_speed_estimate vision_speed: diff --git a/mavros_extras/src/plugins/vision_pose_estimate.cpp b/mavros_extras/src/plugins/vision_pose_estimate.cpp index afe9b105b..da48ca0dc 100644 --- a/mavros_extras/src/plugins/vision_pose_estimate.cpp +++ b/mavros_extras/src/plugins/vision_pose_estimate.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -31,7 +32,7 @@ namespace mavplugin { * */ class VisionPoseEstimatePlugin : public MavRosPlugin, - private TFListenerMixin { + private TF2ListenerMixin { public: VisionPoseEstimatePlugin() : sp_nh("~vision_pose"), @@ -41,29 +42,24 @@ class VisionPoseEstimatePlugin : public MavRosPlugin, void initialize(UAS &uas_) { - bool pose_with_covariance; - bool listen_tf; + bool tf_listen; uas = &uas_; - sp_nh.param("pose_with_covariance", pose_with_covariance, false); - sp_nh.param("listen_tf", listen_tf, false); - sp_nh.param("frame_id", frame_id, "local_origin"); - sp_nh.param("child_frame_id", child_frame_id, "vision"); - sp_nh.param("tf_rate_limit", tf_rate, 50.0); + // tf params + sp_nh.param("tf/listen", tf_listen, false); + sp_nh.param("tf/frame_id", tf_frame_id, "local_origin"); + sp_nh.param("tf/child_frame_id", tf_child_frame_id, "vision"); + sp_nh.param("tf/rate_limit", tf_rate, 50.0); - ROS_DEBUG_STREAM_NAMED("vision_pose", "Vision pose topic type: " << - ((pose_with_covariance) ? "PoseWithCovarianceStamped" : "PoseStamped")); - - if (listen_tf) { - ROS_INFO_STREAM_NAMED("vision_pose", "Listen to vision transform " << frame_id - << " -> " << child_frame_id); - tf_start("VisionPoseTF", &VisionPoseEstimatePlugin::send_vision_transform); + if (tf_listen) { + ROS_INFO_STREAM_NAMED("vision_pose", "Listen to vision transform " << tf_frame_id + << " -> " << tf_child_frame_id); + tf2_start("VisionPoseTF", &VisionPoseEstimatePlugin::transform_cb); } - else if (pose_with_covariance) - vision_sub = sp_nh.subscribe("pose", 10, &VisionPoseEstimatePlugin::vision_cov_cb, this); - else + else { vision_sub = sp_nh.subscribe("pose", 10, &VisionPoseEstimatePlugin::vision_cb, this); + } } const message_map get_rx_handlers() { @@ -71,15 +67,14 @@ class VisionPoseEstimatePlugin : public MavRosPlugin, } private: - friend class TFListenerMixin; + friend class TF2ListenerMixin; ros::NodeHandle sp_nh; UAS *uas; ros::Subscriber vision_sub; - std::string frame_id; - std::string child_frame_id; - + std::string tf_frame_id; + std::string tf_child_frame_id; double tf_rate; ros::Time last_transform_stamp; @@ -105,12 +100,7 @@ class VisionPoseEstimatePlugin : public MavRosPlugin, /** * @brief Send vision estimate transform to FCU position controller */ - void send_vision_transform(const tf::Transform &transform, const ros::Time &stamp) { - tf::Vector3 origin = transform.getOrigin(); - double roll, pitch, yaw; - tf::Matrix3x3 orientation(transform.getBasis()); - orientation.getRPY(roll, pitch, yaw); - + void send_vision_estimate(const ros::Time &stamp, const Eigen::Affine3d &tr) { /** * @warning Issue #60. * This now affects pose callbacks too. @@ -121,9 +111,10 @@ class VisionPoseEstimatePlugin : public MavRosPlugin, } last_transform_stamp = stamp; - auto position = UAS::transform_frame_enu_ned_xyz(origin.x(), origin.y(), origin.z()); - tf::Vector3 rpy = UAS::transform_frame_enu_ned_attitude_rpy(roll, pitch, yaw); - + auto position = UAS::transform_frame_enu_ned(Eigen::Vector3d(tr.translation())); + auto rpy = UAS::quaternion_to_rpy( + UAS::transform_frame_ned_enu(Eigen::Quaterniond(tr.rotation()))); + vision_position_estimate(stamp.toNSec() / 1000, position.x(), position.y(), position.z(), rpy.x(), rpy.y(), rpy.z()); @@ -133,16 +124,18 @@ class VisionPoseEstimatePlugin : public MavRosPlugin, /* common TF listener moved to mixin */ - void vision_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req) { - tf::Transform transform; - poseMsgToTF(req->pose.pose, transform); - send_vision_transform(transform, req->header.stamp); + void transform_cb(const geometry_msgs::TransformStamped &transform) { + Eigen::Affine3d tr; + tf::transformMsgToEigen(transform.transform, tr); + + send_vision_estimate(transform.header.stamp, tr); } void vision_cb(const geometry_msgs::PoseStamped::ConstPtr &req) { - tf::Transform transform; - poseMsgToTF(req->pose, transform); - send_vision_transform(transform, req->header.stamp); + Eigen::Affine3d tr; + tf::poseMsgToEigen(req->pose, tr); + + send_vision_estimate(req->header.stamp, tr); } }; }; // namespace mavplugin From 22ad844e5c29230bca8145eeb37b118075a15920 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 22:03:28 +0300 Subject: [PATCH 15/20] lib #319: remove unused bullet based transform_frame() --- mavros/include/mavros/mavros_uas.h | 159 +---------------------- mavros/src/lib/uas_frame_conversions.cpp | 111 ++-------------- mavros/test/test_frame_conv.cpp | 142 +++++--------------- 3 files changed, 52 insertions(+), 360 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h index 70246dd05..8a4bac6cf 100644 --- a/mavros/include/mavros/mavros_uas.h +++ b/mavros/include/mavros/mavros_uas.h @@ -330,12 +330,6 @@ class UAS { /* -*- frame conversion utilities -*- */ - static tf::Vector3 transform_frame_xyz(double _x, double _y, double _z); - static tf::Quaternion transform_frame_attitude_q(tf::Quaternion qo); - static tf::Vector3 transform_frame_attitude_rpy(double _roll, double _pitch, double _yaw); - static Covariance6x6 transform_frame_covariance_pose6x6(Covariance6x6 &_covariance); - static Covariance3x3 transform_frame_covariance_general3x3(Covariance3x3 &_covariance); - /** * @brief Convert euler angles to quaternion. */ @@ -390,6 +384,11 @@ class UAS { */ static Eigen::Quaterniond transform_frame(const Eigen::Quaterniond &q); + //! @todo implement that function + static Covariance3x3 transform_frame(const Covariance3x3 &cov); + //! @todo implement that function + static Covariance6x6 transform_frame(const Covariance6x6 &cov); + /** * @brief Transform from FCU to ROS frame. */ @@ -406,154 +405,6 @@ class UAS { return transform_frame(in); } - /** - * @brief Function to convert general XYZ values from ENU to NED frames - * @param _x: X coordinate/direction value - * @param _y: Y coordinate/direction value - * @param _z: Z coordinate/direction value - * @return Translated XYZ values in NED frame - */ - static inline tf::Vector3 transform_frame_enu_ned_xyz(double _x, double _y, double _z){ - return transform_frame_xyz(_x, _y, _z); - } - - /** - * @brief Function to convert general XYZ values from NED to ENU frames - * @param _x: X coordinate value - * @param _y: Y coordinate value - * @param _z: Z coordinate value - * @return Translated XYZ values in ENU frame - */ - static inline tf::Vector3 transform_frame_ned_enu_xyz(double _x, double _y, double _z){ - return transform_frame_xyz(_x, _y, _z); - } - - /** - * @brief Function to convert attitude quaternion values from ENU to NED frames - * @param qo: tf::Quaternion format - * @return Rotated Quaternion values in NED frame - */ - static inline tf::Quaternion transform_frame_enu_ned_attitude_q(tf::Quaternion qo){ - return transform_frame_attitude_q(qo); - } - - /** - * @brief Function to convert attitude quaternion values from NED to ENU frames - * @param qo: tf::Quaternion format - * @return Rotated Quaternion values in ENU frame - */ - static inline tf::Quaternion transform_frame_ned_enu_attitude_q(tf::Quaternion qo){ - return transform_frame_attitude_q(qo); - } - - /** - * @brief Function to convert attitude euler angle values from ENU to NED frames - * @param _roll: Roll value - * @param _pitch: Pitch value - * @param _yaw: Yaw value - * @return Rotated RPY angles values in NED frame - */ - static inline tf::Vector3 transform_frame_enu_ned_attitude_rpy(double _roll, double _pitch, double _yaw){ - return transform_frame_attitude_rpy(_roll, _pitch, _yaw); - } - - /** - * @brief Function to convert attitude euler angle values from NED to ENU frames - * @param _roll: Roll value - * @param _pitch: Pitch value - * @param _yaw: Yaw value - * @return Rotated RPY angles values in ENU frame - */ - static inline tf::Vector3 transform_frame_ned_enu_attitude_rpy(double _roll, double _pitch, double _yaw){ - return transform_frame_attitude_rpy(_roll, _pitch, _yaw); - } - - /** - * @brief Function to convert full 6D pose covariance matrix values from ENU to NED frames - * @details Full 6D pose covariance matrix format: a 3D position plus three attitude angles: roll, pitch and yaw. - * @param _covariance: 6x6 double precision covariance matrix - * @return Propagated 6x6 covariance matrix in NED frame, if _covariance[0] != -1 - */ - static inline Covariance6x6 transform_frame_enu_ned_covariance_pose6x6(Covariance6x6 _covariance){ - return transform_frame_covariance_pose6x6(_covariance); - } - - /** - * @brief Function to convert full 6D pose covariance matrix values from NED to ENU frames - * @details Full 6D pose covariance matrix format: a 3D position plus three attitude angles: roll, pitch and yaw. - * @param _covariance: 6x6 double precision covariance matrix - * @return Propagated 6x6 covariance matrix in ENU frame, if _covariance[0] != -1 - */ - static inline Covariance6x6 transform_frame_ned_enu_covariance_pose6x6(Covariance6x6 _covariance){ - return transform_frame_covariance_pose6x6(_covariance); - } - - /** - * Matrix formats for the above funtions: - * - * Cov_matrix = | var_x cov_xy cov_xz cov_xZ cov_xY cov_xX | - * | cov_yx var_y cov_yz cov_yZ cov_yY cov_yX | - * | cov_zx cov_zy var_z cov_zZ cov_zY cov_zX | - * | cov_Zx cov_Zy cov_Zz var_Z cov_ZY cov_ZX | - * | cov_Yx cov_Yy cov_Yz cov_YZ var_Y cov_YX | - * | cov_Xx cov_Xy cov_Xz cov_XZ cov_XY var_X | - * - * Transf_matrix = | 1 0 0 0 0 0 | - * | 0 -1 0 0 0 0 | - * | 0 0 -1 0 0 0 | - * | 0 0 0 1 0 0 | - * | 0 0 0 0 -1 0 | - * | 0 0 0 0 0 -1 | - * - * Compute Covariance matrix in another frame: (according to the law of propagation of covariance) - * - * C' = R * C * R^T - */ - - /** - * @brief Function to convert position, linear acceleration, angular velocity or attitude RPY covariance matrix values from ENU to NED frames - * @param _covariance: 3x3 double precision covariance matrix - * @return Propagated 3x3 covariance matrix in NED frame, if _covariance[0] != -1 - */ - static inline Covariance3x3 transform_frame_covariance_enu_ned_general3x3(Covariance3x3 _covariance){ - return transform_frame_covariance_general3x3(_covariance); - } - - /** - * @brief Function to convert position, linear acceleration, angular velocity or attitude RPY covariance matrix values from NED to ENU frames - * @param _covariance: 3x3 double precision covariance matrix - * @return Propagated 3x3 covariance matrix in ENU frame, if _covariance[0] != -1 - */ - static inline Covariance3x3 transform_frame_covariance_ned_enu_general3x3(Covariance3x3 _covariance){ - return transform_frame_covariance_general3x3(_covariance); - } - - /** - * Matrix formats for the above funtions: - * - * Pos_Cov_matrix = | var_x cov_xy cov_xz | - * | cov_yx var_y cov_yz | - * | cov_zx cov_zy var_z | - * - * Vel_Cov_matrix = | var_vx cov_vxvy cov_vxvz | - * | cov_vyvx var_vy cov_vyvz | - * | cov_vzvx cov_vzvy var_vz | - * - * Att_Cov_matrix = | var_Z cov_ZY cov_ZX | - * | cov_YZ var_Y cov_YX | - * | cov_XZ cov_XY var_X | - * - * Note that for ROS<->ENU frame transformations, the transformation matrix is the same for position and attitude. - * - * Rot_matrix = | 1 0 0 | - * | 0 -1 0 | - * | 0 0 -1 | - * - * Compute Covariance matrix in another frame: (according to the law of propagation of covariance) - * - * C' = T * C * T^t - */ - private: std::recursive_mutex mutex; diff --git a/mavros/src/lib/uas_frame_conversions.cpp b/mavros/src/lib/uas_frame_conversions.cpp index 81495f588..1c9e3ca5b 100644 --- a/mavros/src/lib/uas_frame_conversions.cpp +++ b/mavros/src/lib/uas_frame_conversions.cpp @@ -19,106 +19,7 @@ using namespace mavros; - -tf::Vector3 UAS::transform_frame_xyz(double _x, double _y, double _z) -{ - double x = _x; - double y = -_y; - double z = -_z; - return tf::Vector3(x, y, z); -} - -tf::Quaternion UAS::transform_frame_attitude_q(tf::Quaternion qo) -{ - // XXX temporary - // same reason as for rpy: return old math. -#if 0 - double roll = M_PI, pitch = 0.0, yaw = 0.0; - tf::Quaternion qr = tf::createQuaternionFromRPY(roll, pitch, yaw); - tf::Quaternion qt = qo * qr; - return qt; -#endif - return tf::Quaternion(qo.x(), -qo.y(), -qo.z(), qo.w()); -} - -tf::Vector3 UAS::transform_frame_attitude_rpy(double _roll, double _pitch, double _yaw) -{ - // XXX temporary comment-out new method. - // hand-test in rviz + imu plugin show wrong rotation direction on yaw. - // APM Planner and MAVProxy shows CW, RVIZ CCW => mavros bug. - // return old math. -#if 0 - double roll = _roll + M_PI; - double pitch = _pitch; - double yaw = _yaw; - return tf::Vector3(roll, pitch, yaw); -#endif - return transform_frame_xyz(_roll, _pitch, _yaw); -} - -UAS::Covariance6x6 UAS::transform_frame_covariance_pose6x6(UAS::Covariance6x6 &_covariance) -{ - const UAS::Covariance6x6 rotation = { - 1, 0, 0, 0, 0, 0, - 0, -1, 0, 0, 0, 0, - 0, 0, -1, 0, 0, 0, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, -1, 0, - 0, 0, 0, 0, 0, -1 - }; - - UAS::Covariance6x6 covariance; - UAS::Covariance6x6 temp; // temporary matrix = T * C - - // The transformation matrix in this case is a orthogonal matrix so T = T^t - - /** - * @note According to ROS convention, if one has no estimate for one of the data elements, - * element 0 of the associated covariance matrix to is -1; So no transformation has be applied, - * as the covariance is invalid/unknown; so, it returns the same cov matrix without transformation. - */ - if (_covariance.at(0) != -1) { - // XXX this doesn't multiply matrices correctly. We need Eigen on code! - std::transform(rotation.begin(), rotation.end(), _covariance.begin(), temp.begin(), std::multiplies()); - std::transform(temp.begin(), temp.end(), rotation.begin(), covariance.begin(), std::multiplies()); - return covariance; - } - else { - _covariance.at(0) = -1; - return _covariance; - } -} - -UAS::Covariance3x3 UAS::transform_frame_covariance_general3x3(UAS::Covariance3x3 &_covariance) -{ - const UAS::Covariance3x3 rotation = { - 1, 0, 0, - 0, -1, 0, - 0, 0, -1 - }; - - UAS::Covariance3x3 covariance; - UAS::Covariance3x3 temp; // temporary matrix = T * C - - // The transformation matrix in this case is a orthogonal matrix so T = T^t - - /** - * @note According to ROS convention, if one has no estimate for one of the data elements, - * element 0 of the associated covariance matrix to is -1; So no transformation has be applied, - * as the covariance is invalid/unknown; so, it returns the same cov matrix without transformation. - */ - if (_covariance.at(0) != -1) { - // XXX this doesn't multiply matrices correctly. We need Eigen on code! - std::transform(rotation.begin(), rotation.end(), _covariance.begin(), temp.begin(), std::multiplies()); - std::transform(temp.begin(), temp.end(), rotation.begin(), covariance.begin(), std::multiplies()); - return covariance; - } - else { - return _covariance; - } -} - -// Eigen based functions for test! +// Eigen based functions //! +PI rotation around X (Roll) axis give us ROS or FCU representation static const Eigen::Quaterniond FRAME_ROTATE_Q = UAS::quaternion_from_rpy(M_PI, 0.0, 0.0); @@ -166,3 +67,13 @@ Eigen::Vector3d UAS::transform_frame(const Eigen::Vector3d &vec) { return FRAME_TRANSFORM_VECTOR3 * vec; } + +UAS::Covariance3x3 UAS::transform_frame(const Covariance3x3 &cov) +{ + // not implemented! +} + +UAS::Covariance6x6 UAS::transform_frame(const Covariance6x6 &cov) +{ + // not implemented! +} diff --git a/mavros/test/test_frame_conv.cpp b/mavros/test/test_frame_conv.cpp index ae593a40e..b829c5eda 100644 --- a/mavros/test/test_frame_conv.cpp +++ b/mavros/test/test_frame_conv.cpp @@ -11,65 +11,9 @@ using mavros::UAS; static const double epsilon = 1e-6; +/* -*- test general transform function -*- */ -static std::string log_v(UAS::Covariance6x6 &cov, int n) -{ - std::stringstream cov_stream; - std::string cov_mtrx_ln; - for (int i=n-5; i<=n ; i++) - { - if (i == 35) - cov_stream << cov.at(i); - else - cov_stream << cov.at(i)<< "\t"; - } - cov_mtrx_ln = cov_stream.str(); - return cov_mtrx_ln; -} - -static void log_covariance6x6(UAS::Covariance6x6 &in, UAS::Covariance6x6 &out, UAS::Covariance6x6 &expected) -{ - ROS_INFO("\n\tIn (6x6 Covariance Matrix):\n\t{%s\n\t %s\n\t %s\n\t %s\n\t %s\n\t %s}", - log_v(in,5).c_str(), - log_v(in,11).c_str(), - log_v(in,17).c_str(), - log_v(in,23).c_str(), - log_v(in,29).c_str(), - log_v(in,35).c_str()); - - ROS_INFO("\n\tExpected:\n\t{%s\n\t %s\n\t %s\n\t %s\n\t %s\n\t %s}", - log_v(expected,5).c_str(), - log_v(expected,11).c_str(), - log_v(expected,17).c_str(), - log_v(expected,23).c_str(), - log_v(expected,29).c_str(), - log_v(expected,35).c_str()); - - ROS_INFO("\n\tOut:\n\t{%s\n\t %s\n\t %s\n\t %s\n\t %s\n\t %s}", - log_v(out,5).c_str(), - log_v(out,11).c_str(), - log_v(out,17).c_str(), - log_v(out,23).c_str(), - log_v(out,29).c_str(), - log_v(out,35).c_str()); -} - - -/* -*- test general Vector3 transform function -*- */ - -TEST(BULLET, transform_frame_xyz__123) -{ - tf::Vector3 input(1, 2, 3); - tf::Vector3 expected(1, -2, -3); - - auto out = UAS::transform_frame_xyz(input.x(), input.y(), input.z()); - - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); -} - -TEST(EIGEN, transform_frame__vector3d_123) +TEST(UAS, transform_frame__vector3d_123) { Eigen::Vector3d input(1, 2, 3); Eigen::Vector3d expected(1, -2, -3); @@ -81,12 +25,12 @@ TEST(EIGEN, transform_frame__vector3d_123) EXPECT_NEAR(expected.z(), out.z(), epsilon); } -TEST(BULLET, transform_frame_attitude_q__123) +TEST(UAS, transform_frame__quaterniond_123) { - auto input = tf::createQuaternionFromRPY(1, 2, 3); - auto expected = tf::createQuaternionFromRPY(1, -2, -3); + auto input = UAS::quaternion_from_rpy(1.0, 2.0, 3.0); + auto expected = UAS::quaternion_from_rpy(1.0, -2.0, -3.0); - auto out = UAS::transform_frame_attitude_q(input); + auto out = UAS::transform_frame(input); EXPECT_NEAR(expected.w(), out.w(), epsilon); EXPECT_NEAR(expected.x(), out.x(), epsilon); @@ -94,20 +38,40 @@ TEST(BULLET, transform_frame_attitude_q__123) EXPECT_NEAR(expected.z(), out.z(), epsilon); } -TEST(EIGEN, transform_frame__quaterniond_123) +TEST(UAS, transform_frame__covariance3x3) { - auto input = UAS::quaternion_from_rpy(1.0, 2.0, 3.0); - auto expected = UAS::quaternion_from_rpy(1.0, -2.0, -3.0); +} + +TEST(UAS, transform_frame__covariance6x6) +{ + UAS::Covariance6x6 test = { + 176.75, 1E-6, 1E-6, 1E-6, 4.14, 1E-6, + 1E-6, 1748.8, 1E-6, 2.1, 1E-6, 1E-6, + 1E-6, 1E-6, 11447.14, 1E-6, 4.17, 1E-6, + 1E-6, 2.1, 1E-6, 10001, 1E-6, 1E-6, + 4.14, 1E-6, 4.17, 1E-6, 14111, 1E-6, + 1E-6, 1E-6, 1E-6, 1E-6, 1E-6, 101 + }; + + UAS::Covariance6x6 exp_out = { // XXX Still have to confirm this output + 176.75, -1E-6, -1E-6, -1E-6, -4.14, -1E-6, + -1E-6, 1748.8, -1E-6, -2.1, -1E-6, 1E-6, + -1E-6, -1E-6, 11447.14, 1E-6, 4.17, 1E-6, + -1E-6, -2.1, 1E-6, 10001, 1E-6, 1E-6, + -4.14, 1E-6, 4.17, 1E-6, 14111, 1E-6, + -1E-6, 1E-6, 1E-6, 1E-6, 1E-6, 101 + }; + + auto input = test; + auto expected_out = exp_out; auto out = UAS::transform_frame(input); - EXPECT_NEAR(expected.w(), out.w(), epsilon); - EXPECT_NEAR(expected.x(), out.x(), epsilon); - EXPECT_NEAR(expected.y(), out.y(), epsilon); - EXPECT_NEAR(expected.z(), out.z(), epsilon); + //EXPECT_NEAR(expected_out, out, epsilon); + EXPECT_EQ(expected_out, out); } -TEST(EIGEN, quaternion_from_rpy__check_compatibility) +TEST(UAS, quaternion_from_rpy__check_compatibility) { auto eigen_q = UAS::quaternion_from_rpy(1.0, 2.0, 3.0); auto bt_q = tf::createQuaternionFromRPY(1.0, 2.0, 3.0); @@ -118,7 +82,7 @@ TEST(EIGEN, quaternion_from_rpy__check_compatibility) EXPECT_NEAR(bt_q.z(), eigen_q.z(), epsilon); } -TEST(EIGEN, quaternion_from_rpy__paranoic_check) +TEST(UAS, quaternion_from_rpy__paranoic_check) { auto q1 = UAS::quaternion_from_rpy(1.0, 2.0, 3.0); auto q2 = UAS::quaternion_from_rpy(Eigen::Vector3d(1.0, 2.0, 3.0)); @@ -129,7 +93,7 @@ TEST(EIGEN, quaternion_from_rpy__paranoic_check) EXPECT_NEAR(q1.z(), q2.z(), epsilon); } -TEST(EIGEN, quaternion_to_rpy__123) +TEST(UAS, quaternion_to_rpy__123) { auto q = UAS::quaternion_from_rpy(1.0, 2.0, 3.0); auto rpy = UAS::quaternion_to_rpy(q); @@ -139,42 +103,8 @@ TEST(EIGEN, quaternion_to_rpy__123) EXPECT_NEAR(3.0, rpy.z(), epsilon); } -// XXX: #321 comment out broken transform's before release 0.12 -// after we SHOULD come and fix! -#if 0 - /* -*- test covariance transform -*- */ -TEST(COVARIANCE6X6, transform_frame_covariance_pose6x6_sample1) -{ - UAS::Covariance6x6 test = { - 176.75, 1E-6, 1E-6, 1E-6, 4.14, 1E-6, - 1E-6, 1748.8, 1E-6, 2.1, 1E-6, 1E-6, - 1E-6, 1E-6, 11447.14, 1E-6, 4.17, 1E-6, - 1E-6, 2.1, 1E-6, 10001, 1E-6, 1E-6, - 4.14, 1E-6, 4.17, 1E-6, 14111, 1E-6, - 1E-6, 1E-6, 1E-6, 1E-6, 1E-6, 101 - }; - - UAS::Covariance6x6 exp_out = { // XXX Still have to confirm this output - 176.75, -1E-6, -1E-6, -1E-6, -4.14, -1E-6, - -1E-6, 1748.8, -1E-6, -2.1, -1E-6, 1E-6, - -1E-6, -1E-6, 11447.14, 1E-6, 4.17, 1E-6, - -1E-6, -2.1, 1E-6, 10001, 1E-6, 1E-6, - -4.14, 1E-6, 4.17, 1E-6, 14111, 1E-6, - -1E-6, 1E-6, 1E-6, 1E-6, 1E-6, 101 - }; - - auto input = test; - auto expected_out = exp_out; - - auto out = UAS::transform_frame_covariance_pose6x6(input); - - log_covariance6x6(input, out, expected_out); - EXPECT_EQ(expected_out, out); -} - -#endif int main(int argc, char **argv) { From 09355d29ba6d18b088755c4155715de17a413707 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 23:24:47 +0300 Subject: [PATCH 16/20] lib #319: transform_frame() for Covariance3x3 --- mavros/include/mavros/mavros_uas.h | 16 +++++- mavros/src/lib/uas_frame_conversions.cpp | 18 +++++- mavros/test/test_frame_conv.cpp | 73 ++++++++++++++++-------- 3 files changed, 80 insertions(+), 27 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h index 8a4bac6cf..af2214dc9 100644 --- a/mavros/include/mavros/mavros_uas.h +++ b/mavros/include/mavros/mavros_uas.h @@ -86,6 +86,13 @@ class UAS { //! Type matching rosmsg for covarince 6x6 typedef boost::array Covariance6x6; + //! Eigen::Map for Covariance3x3 + typedef Eigen::Map > EigenMapCovariance3x3; + typedef Eigen::Map > EigenMapConstCovariance3x3; + //! Eigen::Map for Covariance6x6 + typedef Eigen::Map > EigenMapCovariance6x6; + typedef Eigen::Map > EigenMapConstCovariance6x6; + UAS(); ~UAS() {}; @@ -384,9 +391,14 @@ class UAS { */ static Eigen::Quaterniond transform_frame(const Eigen::Quaterniond &q); - //! @todo implement that function + /** + * @brief Transform frame between ROS and FCU. (Covariance3x3) + * + * General function. Please use specialized enu-ned and ned-enu variants. + */ static Covariance3x3 transform_frame(const Covariance3x3 &cov); - //! @todo implement that function + + // XXX TODO implement that function static Covariance6x6 transform_frame(const Covariance6x6 &cov); /** diff --git a/mavros/src/lib/uas_frame_conversions.cpp b/mavros/src/lib/uas_frame_conversions.cpp index 1c9e3ca5b..e5eb79d8e 100644 --- a/mavros/src/lib/uas_frame_conversions.cpp +++ b/mavros/src/lib/uas_frame_conversions.cpp @@ -70,10 +70,24 @@ Eigen::Vector3d UAS::transform_frame(const Eigen::Vector3d &vec) UAS::Covariance3x3 UAS::transform_frame(const Covariance3x3 &cov) { - // not implemented! + Covariance3x3 cov_out_; + EigenMapConstCovariance3x3 cov_in(cov.data()); + EigenMapCovariance3x3 cov_out(cov_out_.data()); + + // code from imu_transformer tf2_sensor_msgs.h + //cov_out = FRAME_ROTATE_Q * cov_in * FRAME_ROTATE_Q.inverse(); + // from comments on github about tf2_sensor_msgs.h + cov_out = cov_in * FRAME_ROTATE_Q; + return cov_out_; } UAS::Covariance6x6 UAS::transform_frame(const Covariance6x6 &cov) { - // not implemented! + Covariance6x6 cov_out_; + EigenMapConstCovariance6x6 cov_in(cov.data()); + EigenMapCovariance6x6 cov_out(cov_out_.data()); + + //! @todo implement me!!! + ROS_ASSERT(false); + return cov_out_; } diff --git a/mavros/test/test_frame_conv.cpp b/mavros/test/test_frame_conv.cpp index b829c5eda..9043ccee4 100644 --- a/mavros/test/test_frame_conv.cpp +++ b/mavros/test/test_frame_conv.cpp @@ -9,7 +9,9 @@ using mavros::UAS; -static const double epsilon = 1e-6; +static const double epsilon = 1e-9; +// gMock has ability to define array matcher, but there problems with that. +// so trying good old for loop /* -*- test general transform function -*- */ @@ -40,36 +42,61 @@ TEST(UAS, transform_frame__quaterniond_123) TEST(UAS, transform_frame__covariance3x3) { + UAS::Covariance3x3 input = {{ + 1.0, 2.0, 3.0, + 4.0, 5.0, 6.0, + 7.0, 8.0, 9.0 + }}; + + /* Calculated as: + * | 1 0 0 | + * input * | 0 -1 0 | + * | 0 0 -1 | + */ + UAS::Covariance3x3 expected = {{ + 1.0, -2.0, -3.0, + 4.0, -5.0, -6.0, + 7.0, -8.0, -9.0 + }}; + + auto out = UAS::transform_frame(input); + + for (size_t idx = 0; idx < expected.size(); idx++) { + SCOPED_TRACE(idx); + EXPECT_NEAR(expected[idx], out[idx], epsilon); + } } +#if 0 +// not implemented TEST(UAS, transform_frame__covariance6x6) { - UAS::Covariance6x6 test = { - 176.75, 1E-6, 1E-6, 1E-6, 4.14, 1E-6, - 1E-6, 1748.8, 1E-6, 2.1, 1E-6, 1E-6, - 1E-6, 1E-6, 11447.14, 1E-6, 4.17, 1E-6, - 1E-6, 2.1, 1E-6, 10001, 1E-6, 1E-6, - 4.14, 1E-6, 4.17, 1E-6, 14111, 1E-6, - 1E-6, 1E-6, 1E-6, 1E-6, 1E-6, 101 - }; - - UAS::Covariance6x6 exp_out = { // XXX Still have to confirm this output - 176.75, -1E-6, -1E-6, -1E-6, -4.14, -1E-6, - -1E-6, 1748.8, -1E-6, -2.1, -1E-6, 1E-6, - -1E-6, -1E-6, 11447.14, 1E-6, 4.17, 1E-6, - -1E-6, -2.1, 1E-6, 10001, 1E-6, 1E-6, - -4.14, 1E-6, 4.17, 1E-6, 14111, 1E-6, - -1E-6, 1E-6, 1E-6, 1E-6, 1E-6, 101 - }; - - auto input = test; - auto expected_out = exp_out; + UAS::Covariance6x6 input = {{ + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, + 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, + 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, + 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, + 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, + 31.0, 32.0, 33.0, 34.0, 35.0, 36.0 + }}; + + UAS::Covariance6x6 expected = {{ + 1.0, -2.0, -3.0, 4.0, -5.0, -6.0, + 7.0, -8.0, -9.0, 10.0, -11.0, -12.0, + 13.0, -14.0, -15.0, 16.0, -17.0, -18.0, + 19.0, -20.0, -21.0, 22.0, -23.0, -24.0, + 25.0, -26.0, -27.0, 28.0, -29.0, -30.0, + 31.0, -32.0, -33.0, 34.0, -35.0, -36.0 + }}; auto out = UAS::transform_frame(input); - //EXPECT_NEAR(expected_out, out, epsilon); - EXPECT_EQ(expected_out, out); + for (size_t idx = 0; idx < expected.size(); idx++) { + SCOPED_TRACE(idx); + EXPECT_NEAR(expected[idx], out[idx], epsilon); + } } +#endif TEST(UAS, quaternion_from_rpy__check_compatibility) { From 1d409ae285f7dddf199834cb755ef93267cb3cce Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 23:33:35 +0300 Subject: [PATCH 17/20] lib #319: use similar names for covariances as eigen vector --- mavros/include/mavros/mavros_uas.h | 22 +++++++++++----------- mavros/src/lib/uas_frame_conversions.cpp | 16 ++++++++-------- mavros/src/plugins/global_position.cpp | 4 ++-- mavros/src/plugins/imu_pub.cpp | 12 ++++++------ mavros/test/test_frame_conv.cpp | 8 ++++---- 5 files changed, 31 insertions(+), 31 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h index af2214dc9..42b31053b 100644 --- a/mavros/include/mavros/mavros_uas.h +++ b/mavros/include/mavros/mavros_uas.h @@ -82,16 +82,16 @@ class UAS { typedef std::unique_lock unique_lock; //! Type matching rosmsg for covariance 3x3 - typedef boost::array Covariance3x3; + typedef boost::array Covariance3d; //! Type matching rosmsg for covarince 6x6 - typedef boost::array Covariance6x6; + typedef boost::array Covariance6d; - //! Eigen::Map for Covariance3x3 - typedef Eigen::Map > EigenMapCovariance3x3; - typedef Eigen::Map > EigenMapConstCovariance3x3; - //! Eigen::Map for Covariance6x6 - typedef Eigen::Map > EigenMapCovariance6x6; - typedef Eigen::Map > EigenMapConstCovariance6x6; + //! Eigen::Map for Covariance3d + typedef Eigen::Map > EigenMapCovariance3d; + typedef Eigen::Map > EigenMapConstCovariance3d; + //! Eigen::Map for Covariance6d + typedef Eigen::Map > EigenMapCovariance6d; + typedef Eigen::Map > EigenMapConstCovariance6d; UAS(); ~UAS() {}; @@ -392,14 +392,14 @@ class UAS { static Eigen::Quaterniond transform_frame(const Eigen::Quaterniond &q); /** - * @brief Transform frame between ROS and FCU. (Covariance3x3) + * @brief Transform frame between ROS and FCU. (Covariance3d) * * General function. Please use specialized enu-ned and ned-enu variants. */ - static Covariance3x3 transform_frame(const Covariance3x3 &cov); + static Covariance3d transform_frame(const Covariance3d &cov); // XXX TODO implement that function - static Covariance6x6 transform_frame(const Covariance6x6 &cov); + static Covariance6d transform_frame(const Covariance6d &cov); /** * @brief Transform from FCU to ROS frame. diff --git a/mavros/src/lib/uas_frame_conversions.cpp b/mavros/src/lib/uas_frame_conversions.cpp index e5eb79d8e..a09d9fbd1 100644 --- a/mavros/src/lib/uas_frame_conversions.cpp +++ b/mavros/src/lib/uas_frame_conversions.cpp @@ -68,11 +68,11 @@ Eigen::Vector3d UAS::transform_frame(const Eigen::Vector3d &vec) return FRAME_TRANSFORM_VECTOR3 * vec; } -UAS::Covariance3x3 UAS::transform_frame(const Covariance3x3 &cov) +UAS::Covariance3d UAS::transform_frame(const Covariance3d &cov) { - Covariance3x3 cov_out_; - EigenMapConstCovariance3x3 cov_in(cov.data()); - EigenMapCovariance3x3 cov_out(cov_out_.data()); + Covariance3d cov_out_; + EigenMapConstCovariance3d cov_in(cov.data()); + EigenMapCovariance3d cov_out(cov_out_.data()); // code from imu_transformer tf2_sensor_msgs.h //cov_out = FRAME_ROTATE_Q * cov_in * FRAME_ROTATE_Q.inverse(); @@ -81,11 +81,11 @@ UAS::Covariance3x3 UAS::transform_frame(const Covariance3x3 &cov) return cov_out_; } -UAS::Covariance6x6 UAS::transform_frame(const Covariance6x6 &cov) +UAS::Covariance6d UAS::transform_frame(const Covariance6d &cov) { - Covariance6x6 cov_out_; - EigenMapConstCovariance6x6 cov_in(cov.data()); - EigenMapCovariance6x6 cov_out(cov_out_.data()); + Covariance6d cov_out_; + EigenMapConstCovariance6d cov_in(cov.data()); + EigenMapCovariance6d cov_out(cov_out_.data()); //! @todo implement me!!! ROS_ASSERT(false); diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp index 5935aea85..e09b640c8 100644 --- a/mavros/src/plugins/global_position.cpp +++ b/mavros/src/plugins/global_position.cpp @@ -245,8 +245,8 @@ class GlobalPositionPlugin : public MavRosPlugin { tf::quaternionTFToMsg(uas->get_attitude_orientation(), pose_cov->pose.pose.orientation); // Use ENU covariance to build XYZRPY covariance - Eigen::Map > gps_cov(fix->position_covariance.data()); - Eigen::Map > cov_out(pose_cov->pose.covariance.data()); + UAS::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data()); + UAS::EigenMapCovariance6d cov_out(pose_cov->pose.covariance.data()); cov_out << gps_cov(0, 0) , gps_cov(0, 1) , gps_cov(0, 2) , 0.0 , 0.0 , 0.0 , gps_cov(1, 0) , gps_cov(1, 1) , gps_cov(1, 2) , 0.0 , 0.0 , 0.0 , diff --git a/mavros/src/plugins/imu_pub.cpp b/mavros/src/plugins/imu_pub.cpp index ae9bad890..ef0969f9c 100644 --- a/mavros/src/plugins/imu_pub.cpp +++ b/mavros/src/plugins/imu_pub.cpp @@ -115,15 +115,15 @@ class IMUPubPlugin : public MavRosPlugin { bool has_scaled_imu; bool has_att_quat; Eigen::Vector3d linear_accel_vec; - UAS::Covariance3x3 linear_acceleration_cov; - UAS::Covariance3x3 angular_velocity_cov; - UAS::Covariance3x3 orientation_cov; - UAS::Covariance3x3 unk_orientation_cov; - UAS::Covariance3x3 magnetic_cov; + UAS::Covariance3d linear_acceleration_cov; + UAS::Covariance3d angular_velocity_cov; + UAS::Covariance3d orientation_cov; + UAS::Covariance3d unk_orientation_cov; + UAS::Covariance3d magnetic_cov; /* -*- helpers -*- */ - void setup_covariance(UAS::Covariance3x3 &cov, double stdev) { + void setup_covariance(UAS::Covariance3d &cov, double stdev) { std::fill(cov.begin(), cov.end(), 0.0); if (stdev == 0.0) cov[0] = -1.0; diff --git a/mavros/test/test_frame_conv.cpp b/mavros/test/test_frame_conv.cpp index 9043ccee4..e497dd838 100644 --- a/mavros/test/test_frame_conv.cpp +++ b/mavros/test/test_frame_conv.cpp @@ -42,7 +42,7 @@ TEST(UAS, transform_frame__quaterniond_123) TEST(UAS, transform_frame__covariance3x3) { - UAS::Covariance3x3 input = {{ + UAS::Covariance3d input = {{ 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 @@ -53,7 +53,7 @@ TEST(UAS, transform_frame__covariance3x3) * input * | 0 -1 0 | * | 0 0 -1 | */ - UAS::Covariance3x3 expected = {{ + UAS::Covariance3d expected = {{ 1.0, -2.0, -3.0, 4.0, -5.0, -6.0, 7.0, -8.0, -9.0 @@ -71,7 +71,7 @@ TEST(UAS, transform_frame__covariance3x3) // not implemented TEST(UAS, transform_frame__covariance6x6) { - UAS::Covariance6x6 input = {{ + UAS::Covariance6d input = {{ 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, @@ -80,7 +80,7 @@ TEST(UAS, transform_frame__covariance6x6) 31.0, 32.0, 33.0, 34.0, 35.0, 36.0 }}; - UAS::Covariance6x6 expected = {{ + UAS::Covariance6d expected = {{ 1.0, -2.0, -3.0, 4.0, -5.0, -6.0, 7.0, -8.0, -9.0, 10.0, -11.0, -12.0, 13.0, -14.0, -15.0, 16.0, -17.0, -18.0, From 8b1e1df8b6512ee8b9af4843e5d4f9c198978d3e Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Fri, 10 Jul 2015 23:47:50 +0300 Subject: [PATCH 18/20] plugin: Use UAS::syncronized_header() for reduce LOC. --- mavros/src/plugins/global_position.cpp | 7 ++----- mavros/src/plugins/imu_pub.cpp | 19 +++++-------------- mavros_extras/src/plugins/distance_sensor.cpp | 3 +-- 3 files changed, 8 insertions(+), 21 deletions(-) diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp index e09b640c8..f87acd740 100644 --- a/mavros/src/plugins/global_position.cpp +++ b/mavros/src/plugins/global_position.cpp @@ -122,8 +122,7 @@ class GlobalPositionPlugin : public MavRosPlugin { auto fix = boost::make_shared(); - fix->header.frame_id = frame_id; - fix->header.stamp = uas->synchronise_stamp(raw_gps.time_usec); + fix->header = uas->synchronized_header(frame_id, raw_gps.time_usec); fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; if (raw_gps.fix_type > 2) @@ -186,9 +185,7 @@ class GlobalPositionPlugin : public MavRosPlugin { auto relative_alt = boost::make_shared(); auto compass_heading = boost::make_shared(); - std_msgs::Header header; - header.frame_id = frame_id; - header.stamp = uas->synchronise_stamp(gpos.time_boot_ms); + auto header = uas->synchronized_header(frame_id, gpos.time_boot_ms); // Global position fix fix->header = header; diff --git a/mavros/src/plugins/imu_pub.cpp b/mavros/src/plugins/imu_pub.cpp index ef0969f9c..b00906c7e 100644 --- a/mavros/src/plugins/imu_pub.cpp +++ b/mavros/src/plugins/imu_pub.cpp @@ -146,15 +146,6 @@ class IMUPubPlugin : public MavRosPlugin { uas->update_attitude_imu(orientation, angular_velocity, linear_acceleration); } - //! make message header with syncronized stamp - template - inline std_msgs::Header make_header(T mavlink_msg_time) { - std_msgs::Header header; - header.frame_id = frame_id; - header.stamp = uas->synchronise_stamp(mavlink_msg_time); - return header; - } - //! fill and publish imu/data message void publish_imu_data( uint32_t time_boot_ms, @@ -164,7 +155,7 @@ class IMUPubPlugin : public MavRosPlugin { auto imu_msg = boost::make_shared(); // fill - imu_msg->header = make_header(time_boot_ms); + imu_msg->header = uas->synchronized_header(frame_id, time_boot_ms); tf::quaternionEigenToMsg(orientation, imu_msg->orientation); tf::vectorEigenToMsg(gyro, imu_msg->angular_velocity); @@ -259,7 +250,7 @@ class IMUPubPlugin : public MavRosPlugin { ROS_INFO_COND_NAMED(!has_hr_imu, "imu", "IMU: High resolution IMU detected!"); has_hr_imu = true; - auto header = make_header(imu_hr.time_usec); + auto header = uas->synchronized_header(frame_id, imu_hr.time_usec); //! @todo make more paranoic check of HIGHRES_IMU.fields_updated @@ -307,7 +298,7 @@ class IMUPubPlugin : public MavRosPlugin { mavlink_raw_imu_t imu_raw; mavlink_msg_raw_imu_decode(msg, &imu_raw); - auto header = make_header(imu_raw.time_usec); + auto header = uas->synchronized_header(frame_id, imu_raw.time_usec); //! @note APM send SCALED_IMU data as RAW_IMU @@ -345,7 +336,7 @@ class IMUPubPlugin : public MavRosPlugin { mavlink_scaled_imu_t imu_raw; mavlink_msg_scaled_imu_decode(msg, &imu_raw); - auto header = make_header(imu_raw.time_boot_ms); + auto header = uas->synchronized_header(frame_id, imu_raw.time_boot_ms); auto gyro = UAS::transform_frame_ned_enu( Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC); @@ -368,7 +359,7 @@ class IMUPubPlugin : public MavRosPlugin { mavlink_scaled_pressure_t press; mavlink_msg_scaled_pressure_decode(msg, &press); - auto header = make_header(press.time_boot_ms); + auto header = uas->synchronized_header(frame_id, press.time_boot_ms); auto temp_msg = boost::make_shared(); temp_msg->header = header; diff --git a/mavros_extras/src/plugins/distance_sensor.cpp b/mavros_extras/src/plugins/distance_sensor.cpp index af0095771..e5f54dbc5 100644 --- a/mavros_extras/src/plugins/distance_sensor.cpp +++ b/mavros_extras/src/plugins/distance_sensor.cpp @@ -202,8 +202,7 @@ class DistanceSensorPlugin : public MavRosPlugin { auto range = boost::make_shared(); - range->header.stamp = uas->synchronise_stamp(dist_sen.time_boot_ms); - range->header.frame_id = sensor->frame_id; + range->header = uas->synchronized_header(sensor->frame_id, dist_sen.time_boot_ms); range->min_range = dist_sen.min_distance * 1E-2; // in meters range->max_range = dist_sen.max_distance * 1E-2; From ea8449c454a305234a4e09eaaf07232b129d3e7f Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sat, 11 Jul 2015 00:04:03 +0300 Subject: [PATCH 19/20] extras: distance_sensor #71: Purt to TF2. --- mavros_extras/src/plugins/distance_sensor.cpp | 27 ++++++++++--------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/mavros_extras/src/plugins/distance_sensor.cpp b/mavros_extras/src/plugins/distance_sensor.cpp index e5f54dbc5..50e7bf853 100644 --- a/mavros_extras/src/plugins/distance_sensor.cpp +++ b/mavros_extras/src/plugins/distance_sensor.cpp @@ -13,14 +13,13 @@ * in the top-level LICENSE file of the mavros repository. * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ +#include #include #include #include - -#include +#include #include -#include namespace mavplugin { class DistanceSensorPlugin; @@ -145,8 +144,6 @@ class DistanceSensorPlugin : public MavRosPlugin { ros::NodeHandle dist_nh; UAS *uas; - tf::TransformBroadcaster tf_broadcaster; - std::unordered_map sensor_map; /* -*- low-level send -*- */ @@ -225,20 +222,24 @@ class DistanceSensorPlugin : public MavRosPlugin { if (sensor->send_tf) { /* variables init */ - tf::Transform transform; + // XXX #319 auto rpy = UAS::sensor_orientation_matching(static_cast(dist_sen.orientation)); + // how it can work, if rpy in degrees, not radians? auto q = tf::createQuaternionFromRPY(rpy.x(), rpy.y(), rpy.z()); + geometry_msgs::TransformStamped transform; + + // @TSC21 revisit that please! + // In TF1 code transform: sensor -> fcu is that true? + transform.header = range->header; + transform.child_frame_id = "fcu"; + /* rotation and position set */ - transform.setRotation(q); - transform.setOrigin(sensor->position); + tf::quaternionTFToMsg(q, transform.transform.rotation); + tf::vector3TFToMsg(sensor->position, transform.transform.translation); /* transform broadcast */ - tf_broadcaster.sendTransform( - tf::StampedTransform( - transform, - range->header.stamp, - "fcu", range->header.frame_id)); + uas->tf2_broadcaster.sendTransform(transform); } sensor->pub.publish(range); From 41631cd921d5c6d5afcb7319b9e7028563d3dfc9 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sat, 11 Jul 2015 00:34:39 +0300 Subject: [PATCH 20/20] fix #71: replace depend tf to tf2_ros. --- mavros/CMakeLists.txt | 4 ++-- mavros/package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt index 637682dcd..3ddf15025 100644 --- a/mavros/CMakeLists.txt +++ b/mavros/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs std_msgs std_srvs - tf + tf2_ros angles libmavconn rosconsole_bridge @@ -137,7 +137,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include LIBRARIES mavros - CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs std_msgs tf geometry_msgs libmavconn message_runtime eigen_conversions + CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs std_msgs tf2_ros geometry_msgs libmavconn message_runtime eigen_conversions DEPENDS Boost Eigen ) diff --git a/mavros/package.xml b/mavros/package.xml index c0cafa7c3..6dfedf6f7 100644 --- a/mavros/package.xml +++ b/mavros/package.xml @@ -31,7 +31,7 @@ sensor_msgs std_msgs std_srvs - tf + tf2_ros mavlink angles libmavconn