Merge pull request #35 from AutoModality/AM-232

feat: add more functionality

Author: Aaron Roller

include/vb_util_lib/imu_class.h

@@ -45,9 +45,29 @@ namespace am
   class ImuClass
   {
   public:
+	/*
+	 * @brief default constructor. the user needs to subscribe to imu topics
+	 */
     ImuClass();
+
+    /*
+     * @brief constructor
+     */
     ImuClass(int imu_pot_size);
+
+    /*
+     * @brief constructor for PX4 and DJI imu
+     */
     ImuClass(ros::NodeHandle &nh, int imu_pot_size = 400);
+
+    /*
+     * @brief constructor for customized imu source
+     */
+    ImuClass(ros::NodeHandle &nh, const std::string &imu_topic, int imu_pot_size = 400);
+
+    /*
+     * @brief destructor
+     */
     ~ImuClass();
 
     void addImu(const sensor_msgs::Imu &imu);

include/vb_util_lib/rotate.h

@@ -6,6 +6,8 @@
 #include <tf/tf.h>
 #include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/Point32.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 namespace am
 {
@@ -70,13 +72,21 @@ namespace am
 
     static void rotate(double &x, double &y, double &z, geometry_msgs::Quaternion &q_rot);
 
+    static void rotate(double &x, double &y, double &z, const geometry_msgs::Quaternion &q_rot);
+
     static void rotate(geometry_msgs::Point32 &p, const geometry_msgs::Quaternion &q_rot);
 
+
     /*
      * @brief transforms a point with the passed transformed stamped
      */
     static void transform(geometry_msgs::Point32 &p_in, const geometry_msgs::TransformStamped &tranform);
 
+    /*
+     * @brief transforms a pose with another pose
+     */
+    static void transform(const geometry_msgs::Pose &src, geometry_msgs::Pose &des);
+
     /*
      * @brief inverse transform
      */
@@ -91,6 +101,9 @@ namespace am
     //Static method to convert quaternion to rpy
     static void getRPY(tf::Quaternion &q, double &roll, double &pitch, double &yaw);
 
+    //Static method to convert pose to rpy in geometry_msgs::Vector3
+    static geometry_msgs::Vector3 getRPY(const geometry_msgs::Pose &pose);
+
     //Static method to get Yaw from geometry_msgs Quaternion
     static double getYaw(const geometry_msgs::Quaternion &q);
 

src/vb_util_lib/imu_class.cpp

@@ -11,11 +11,18 @@
 
 namespace am
 {
+
+ImuClass::ImuClass(ros::NodeHandle &nh, const std::string &imu_topic, int imu_pot_size): imu_pot_size_(imu_pot_size), imu_cnt_(-1)
+{
+	nh_ = nh;
+	px4_imu_sub_= nh_.subscribe<sensor_msgs::Imu>(imu_topic, 100, &ImuClass::imuCB, this);
+	imu_pot_ = std::vector<sensor_msgs::Imu>(imu_pot_size);
+}
+
 ImuClass::ImuClass(ros::NodeHandle &nh, int imu_pot_size): imu_cnt_(-1)
 {
 	nh_ = nh;
 	dji_imu_sub_ = nh_.subscribe<sensor_msgs::Imu>(am_topics::DJI_SDK_IMU, 100, &ImuClass::imuCB, this);
-	//TODO: use PX4_IMU_TOPIC from am_topics
 	px4_imu_sub_ = nh_.subscribe<sensor_msgs::Imu>(am_topics::MAVROS_IMU_DATA, 100, &ImuClass::imuCB, this);
 	imu_pot_size_ = imu_pot_size;
 	imu_pot_ = std::vector<sensor_msgs::Imu>(imu_pot_size_);

src/vb_util_lib/rotate.cpp

@@ -339,6 +339,16 @@ namespace am
     m.getRPY(roll, pitch, yaw);
   }
 
+  geometry_msgs::Vector3 Rotate::getRPY(const geometry_msgs::Pose &pose)
+  {
+  	tf2::Quaternion tf2_q;
+  	tf2::fromMsg(pose.orientation, tf2_q);
+  	geometry_msgs::Vector3 rpy;
+
+  	tf2::Matrix3x3(tf2_q).getRPY(rpy.x, rpy.y, rpy.z);
+  	return rpy;
+  }
+
   void Rotate::rotate(double &x, double &y, double &z, geometry_msgs::Quaternion &q_rot)
   {
 	  double old_x = x;
@@ -351,6 +361,18 @@ namespace am
 
   }
 
+  void Rotate::rotate(double &x, double &y, double &z, const geometry_msgs::Quaternion &q_rot)
+  {
+  	double old_x = x;
+  	double old_y = y;
+  	double old_z = z;
+
+  	x = q_rot.w*q_rot.w*old_x + 2*q_rot.y*q_rot.w*old_z - 2*q_rot.z*q_rot.w*old_y + q_rot.x*q_rot.x*old_x + 2*q_rot.y*q_rot.x*old_y + 2*q_rot.z*q_rot.x*old_z - q_rot.z*q_rot.z*old_x - q_rot.y*q_rot.y*old_x;
+  	y = 2*q_rot.x*q_rot.y*old_x + q_rot.y*q_rot.y*old_y + 2*q_rot.z*q_rot.y*old_z + 2*q_rot.w*q_rot.z*old_x - q_rot.z*q_rot.z*old_y + q_rot.w*q_rot.w*old_y - 2*q_rot.x*q_rot.w*old_z - q_rot.x*q_rot.x*old_y;
+  	z = 2*q_rot.x*q_rot.z*old_x + 2*q_rot.y*q_rot.z*old_y + q_rot.z*q_rot.z*old_z - 2*q_rot.w*q_rot.y*old_x - q_rot.y*q_rot.y*old_z + 2*q_rot.w*q_rot.x*old_y - q_rot.x*q_rot.x*old_z + q_rot.w*q_rot.w*old_z;
+
+  }
+
   void Rotate::rotate(geometry_msgs::Point32 &p, const geometry_msgs::Quaternion &q_rot)
   {
 	  geometry_msgs::Point32 old_p = p;
@@ -462,6 +484,21 @@ namespace am
     return angle;
   }
 
+  void Rotate::transform(const geometry_msgs::Pose &src, geometry_msgs::Pose &des)
+  {
+  	des.position.x = des.position.x - src.position.x;
+  	des.position.y = des.position.y - src.position.y;
+  	des.position.z = des.position.z - src.position.z;
+
+  	geometry_msgs::Vector3 rpy = Rotate::getRPY(src);
+  	rpy.x *= -1;
+  	rpy.y *= -1;
+  	rpy.z *= -1;
+  	//ROS_INFO("Q:[%f,%f,%f,%f]", src.orientation.x, src.orientation.y, src.orientation.z, src.orientation.w);
+  	Rotate::rotate(des.position.x, des.position.y, des.position.z, toQuaternionMsg(rpy.x, rpy.y, rpy.z));
+
+  }
+
 }