From dbadd75f288611a8ec316700ea111c7c4aea3886 Mon Sep 17 00:00:00 2001 From: edkoch Date: Mon, 17 Aug 2020 11:01:35 -0700 Subject: [PATCH] feat: Changed topic name. Added new method to transformer. --- include/vb_util_lib/topics.h | 2 +- include/vb_util_lib/transformer.h | 24 +++++++++++++++++------- src/vb_util_lib/kinematics.cpp | 1 - src/vb_util_lib/topics.cpp | 2 +- src/vb_util_lib/transformer.cpp | 22 ++++++++++++++++++++++ 5 files changed, 41 insertions(+), 10 deletions(-) diff --git a/include/vb_util_lib/topics.h b/include/vb_util_lib/topics.h index 5832956..518d5f2 100644 --- a/include/vb_util_lib/topics.h +++ b/include/vb_util_lib/topics.h @@ -173,7 +173,7 @@ class am_topics static constexpr char CTRL_VX_RTL_ACTION[] = "/ctrl/vx/rtl_action"; static constexpr char CTRL_VX_TARGET_CURRENT_ENU[] = "/ctrl/vx/target/currentENU"; static constexpr char CTRL_VX_VEHICLE_CURRENTENU[] = "/ctrl/vx/vehicle/currentENU"; - static constexpr char CTRL_VX_VELOCITY[] = "/ctrl/vx/velocity"; + static constexpr char CTRL_VX_VELOCITY_CMD[] = "/ctrl/vx/velocity_cmd"; static constexpr char CTRL_VX_WAYPOINT_NAV_ACTION [] = "/ctrl/vx/waypoint_nav_action"; static constexpr char CTRL_FEATURE_STATUS_LIST[] = "/ctrl/feature_status_list"; diff --git a/include/vb_util_lib/transformer.h b/include/vb_util_lib/transformer.h index dd41f00..aa22868 100644 --- a/include/vb_util_lib/transformer.h +++ b/include/vb_util_lib/transformer.h @@ -8,6 +8,8 @@ #ifndef VISBOX_PACKAGES_COMMON_UTILS_UTIL_LIB_INCLUDE_VB_UTIL_LIB_TRANSFORMER_H_ #define VISBOX_PACKAGES_COMMON_UTILS_UTIL_LIB_INCLUDE_VB_UTIL_LIB_TRANSFORMER_H_ +#include + #include #include #include @@ -15,6 +17,9 @@ #include #include +#include +#include + namespace am { #define DEFAULT_TRANSFORM_WAIT 0.07 @@ -27,21 +32,26 @@ class Transformer tf2_ros::Buffer& tfBuffer(); - // Used to fetch the latest transform between two frames - // Return true if it works and false otherwise + /// Used to fetch the latest transform between two frames + /// Return true if it works and false otherwise bool getTransform(const std::string target, const std::string source, geometry_msgs::TransformStamped& transform, double wait = -1.0, bool debug = false); - // Used to fetch the latest transform between two frames - // Checks the initialized flag and will not call the lookup if it is true. - // Otherwise it attempts to lookup the transform and set the initialized flag appropriately. - // Return true if the transform is valid and false otherwise + /// Used to fetch the latest transform between two frames + /// Checks the initialized flag and will not call the lookup if it is true. + /// Otherwise it attempts to lookup the transform and set the initialized flag appropriately. + /// Return true if the transform is valid and false otherwise bool getTransform(bool& initialized_flag, const std::string target, const std::string source, geometry_msgs::TransformStamped& transform, double wait = -1.0, bool debug = false); - // Used to rotate the "orientation" coordinates by the "rotation" coordinates + /// Used to rotate the "orientation" coordinates by the "rotation" coordinates void rotateOrientation(geometry_msgs:: Quaternion& orientation, geometry_msgs:: Quaternion& rotation, bool debug = false); + /// Used to rotate a vector from source frame to target frame. + /// Returns false if failed, + bool rotateVector(const std::string target_frame, const std::string source_frame, + Eigen::Vector3d& target_vector, const Eigen::Vector3d& source_vector, bool debug = false); + private: // transform buffer management. Use factory creation method to auto create the tfBuffer // later make this a static to save space and make more efficient diff --git a/src/vb_util_lib/kinematics.cpp b/src/vb_util_lib/kinematics.cpp index 861a85a..f0ec428 100644 --- a/src/vb_util_lib/kinematics.cpp +++ b/src/vb_util_lib/kinematics.cpp @@ -206,7 +206,6 @@ void Kinematics::setPosition(const ros::Time& update_time, double x, double y, d ROS_INFO("Kinematics Velocity standard[%s : %0.3f %0.3f, %0.3f] body[%s : %0.3f %0.3f, %0.3f]", frame_id_.c_str(), velLinear_[0], velLinear_[1], velLinear_[2], drone_ENU, vel_debug[0], vel_debug[1], vel_debug[2]); - } } diff --git a/src/vb_util_lib/topics.cpp b/src/vb_util_lib/topics.cpp index 3fd893e..d41e4be 100644 --- a/src/vb_util_lib/topics.cpp +++ b/src/vb_util_lib/topics.cpp @@ -103,7 +103,7 @@ constexpr char am_topics::CTRL_VX_PROXIMITY[]; constexpr char am_topics::CTRL_VX_RTL_ACTION[]; constexpr char am_topics::CTRL_VX_TARGET_CURRENT_ENU[]; constexpr char am_topics::CTRL_VX_VEHICLE_CURRENTENU[]; -constexpr char am_topics::CTRL_VX_VELOCITY[]; +constexpr char am_topics::CTRL_VX_VELOCITY_CMD[]; constexpr char am_topics::CTRL_VX_WAYPOINT_NAV_ACTION[]; constexpr char am_topics::CTRL_FEATURE_STATUS_LIST[]; diff --git a/src/vb_util_lib/transformer.cpp b/src/vb_util_lib/transformer.cpp index 45de02c..758ff68 100644 --- a/src/vb_util_lib/transformer.cpp +++ b/src/vb_util_lib/transformer.cpp @@ -90,4 +90,26 @@ void Transformer::rotateOrientation(geometry_msgs::Quaternion& orientation, geom } } +bool Transformer::rotateVector(const std::string target_frame, + const std::string source_frame, Eigen::Vector3d& target_vector, + const Eigen::Vector3d& source_vector, bool debug) { + + geometry_msgs::TransformStamped ts; + if (!getTransform(target_frame, source_frame, ts, true)) + { + ROS_ERROR("Kinematics::getLinearVelocity : No transform from[%s] to[%s]", source_frame.c_str(), target_frame.c_str()); + return false; + } + + Eigen::Quaterniond rot; + rot.x() = ts.transform.rotation.x; + rot.y() = ts.transform.rotation.y; + rot.z() = ts.transform.rotation.z; + rot.w() = ts.transform.rotation.w; + + target_vector = rot * source_vector; + + return true; +} + } /* namespace am */