Merge pull request #48 from AutoModality/BB-424/collision_avoidance

Bb 424/collision avoidance

Author: edkoch

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";

include/vb_util_lib/transformer.h

@@ -8,13 +8,18 @@
 #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 <string>
+
 #include <ros/ros.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/LinearMath/Transform.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 #include <geometry_msgs/Quaternion.h>
 
+#include <Eigen/Geometry>
+#include <Eigen/StdVector>
+
 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

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]);
-
 	}
 }
 

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[];

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 */