adding velocity publishers and filters

include/htc_vive_tracker_alg_node.h

@@ -38,6 +38,8 @@
 #include "geometry_msgs/PoseStamped.h"
 
 #include <nav_msgs/Odometry.h>
+
+#include <boost/circular_buffer.hpp>
 // [publisher subscriber headers]
  
 // [service client headers]
@@ -52,7 +54,8 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
 {
   private:
     // [publisher attributes]
-    ros::Publisher vo_publisher_;
+    ros::Publisher vo_t1_publisher_;
+    ros::Publisher vo_c1_publisher_;
     ros::Publisher pose_publisher_;
     //Transformation from base to world. In this case, WAM to CHAPERONE
  
@@ -79,7 +82,18 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
     uint32_t haptic_pulse_strength_;
     bool publish_hmd_;
     bool frame_names_set;
-
+    float controller_v_x;
+    float controller_v_y;
+    float controller_v_z;
+    float tracker_v_x;
+    float tracker_v_y;
+    float tracker_v_z;
+    boost::circular_buffer<float> C_x_buffer;
+    boost::circular_buffer<float> C_y_buffer;
+    boost::circular_buffer<float> C_z_buffer;
+    boost::circular_buffer<float> T_x_buffer;
+    boost::circular_buffer<float> T_y_buffer;
+    boost::circular_buffer<float> T_z_buffer;
     std::string target_frame_name_;
 
     ros::ServiceServer trigger_pulse_server_;
@@ -158,7 +172,7 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
     
     tf::Quaternion ApplyZRotation(const tf::Quaternion & orig);
 
-    nav_msgs::Odometry CreateOdometryFromPoseVel(const geometry_msgs::Pose & pose, const Velocity & vel);
+    nav_msgs::Odometry CreateOdometryFromPoseVel(const geometry_msgs::Pose & pose, const Velocity & vel, const std::string &device_name);
     
 };
 

src/htc_vive_tracker_alg_node.cpp

@@ -8,7 +8,7 @@ HtcViveTrackerAlgNode::HtcViveTrackerAlgNode(void) :
 	this->public_node_handle_.getParam("loop_rate", loop_rate);
    } else {
 	ROS_INFO ("Using default loop_rate!");
-	loop_rate = 20;
+	loop_rate = 50;
    }
 
    this->setRate(loop_rate);
@@ -32,11 +32,24 @@ HtcViveTrackerAlgNode::HtcViveTrackerAlgNode(void) :
     }
     
 
-
+    controller_v_x=0;
+    controller_v_y=0;
+    controller_v_z=0;
+    tracker_v_x=0;
+    tracker_v_y=0;
+    tracker_v_z=0;
+    
+    this->C_x_buffer.set_capacity(5);  
+    this->C_y_buffer.set_capacity(5);  
+    this->C_z_buffer.set_capacity(5);  
+    this->T_x_buffer.set_capacity(5);  
+    this->T_y_buffer.set_capacity(5);  
+    this->T_z_buffer.set_capacity(5);  
 
   // [init publishers]
   
-    vo_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("vo",100);
+    vo_t1_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("vo_tracker_1",100);
+    vo_c1_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("vo_controller_1",100);
     pose_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("new_pose",100);
   
   // [init subscribers]
@@ -158,32 +171,31 @@ void HtcViveTrackerAlgNode::BroadcastPoseRotated(const std::string & device_name
 	
     if (this->alg_.GetDevicePositionQuaternion(device_name,pose,quaternion)) {
 
-
-   	tf::Quaternion q = tf::Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
-	
-	//Apply the necessary rotations so that the coordinate system is the one decided at IRI
-	q = this -> ApplyRotationForIRIStandardCoordinates(q);
-
-  if (device_name == "tracker_1" || device_name == "tracker_2" || device_name == "tracker_3" ){
-      q = this ->ApplyZRotation(q);
-  }
-
-	this->transform_stamped_.header.stamp = ros::Time::now();
-	this->transform_stamped_.header.frame_id = this->alg_.WORLD_NAME;
-    transform_stamped_.child_frame_id = device_name;
-	transform_stamped_.transform.translation.x = pose[0];
-	transform_stamped_.transform.translation.y = pose[1];
-	transform_stamped_.transform.translation.z = pose[2];
-	
-	transform_stamped_.transform.rotation.x = q.x();
-	transform_stamped_.transform.rotation.y = q.y();
-	transform_stamped_.transform.rotation.z = q.z();
-	transform_stamped_.transform.rotation.w = q.w();
-
-
-	tf_broadcaster.sendTransform(transform_stamped_);
-    //If the device is the one we want to follow, also publish the Odometry
-    if (this->frame_names_set && device_name == this->target_frame_name_){
+      tf::Quaternion q = tf::Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
+
+      //Apply the necessary rotations so that the coordinate system is the one decided at IRI
+      q = this -> ApplyRotationForIRIStandardCoordinates(q);
+
+      if (device_name == "tracker_1" || device_name == "tracker_2" || device_name == "tracker_3" ){
+        q = this ->ApplyZRotation(q);
+      }
+
+      this->transform_stamped_.header.stamp = ros::Time::now();
+      this->transform_stamped_.header.frame_id = this->alg_.WORLD_NAME;
+      this->transform_stamped_.child_frame_id = device_name;
+      this->transform_stamped_.transform.translation.x = pose[0];
+      this->transform_stamped_.transform.translation.y = pose[1];
+      this->transform_stamped_.transform.translation.z = pose[2];
+      this->transform_stamped_.transform.rotation.x = q.x();
+      this->transform_stamped_.transform.rotation.y = q.y();
+      this->transform_stamped_.transform.rotation.z = q.z();
+      this->transform_stamped_.transform.rotation.w = q.w();
+      tf_broadcaster.sendTransform(transform_stamped_);
+
+      //If the device is the one we want to follow, also publish the Odometry
+      if (this->frame_names_set){
+      if (device_name == "tracker_1" || device_name == "controller_1" ){
+      //if (this->frame_names_set && device_name == this->target_frame_name_){
         geometry_msgs::Pose device_pose;
         device_pose.position.x = pose[0];
         device_pose.position.y = pose[1];
@@ -192,10 +204,16 @@ void HtcViveTrackerAlgNode::BroadcastPoseRotated(const std::string & device_name
         device_pose.orientation.y = q.y();
         device_pose.orientation.z = q.z();
         device_pose.orientation.w = q.w();
-        Velocity device_vel = this->alg_.GetDeviceVelocity(this->target_frame_name_);
-        nav_msgs::Odometry current_vo = this->CreateOdometryFromPoseVel(device_pose, device_vel);
-        this->vo_publisher_.publish(current_vo);
-    }
+        Velocity device_vel = this->alg_.GetDeviceVelocity(device_name);
+        nav_msgs::Odometry current_vo = this->CreateOdometryFromPoseVel(device_pose, device_vel, device_name);
+        if (device_name == "tracker_1") 
+          this->vo_t1_publisher_.publish(current_vo);
+        else if (device_name == "controller_1")
+          this->vo_c1_publisher_.publish(current_vo);
+        else
+          std::cout << "This is not possible" << std::endl;
+      }
+      }
     }
 
 }
@@ -258,15 +276,60 @@ tf::Quaternion HtcViveTrackerAlgNode::ApplyZRotation(const tf::Quaternion & orig
 }
 
 
-nav_msgs::Odometry HtcViveTrackerAlgNode::CreateOdometryFromPoseVel(const geometry_msgs::Pose & pose, const Velocity & vel){
+nav_msgs::Odometry HtcViveTrackerAlgNode::CreateOdometryFromPoseVel(const geometry_msgs::Pose & pose, const Velocity & vel, const std::string &device_name){
     
     geometry_msgs::TwistWithCovariance twist_msg;
-    twist_msg.twist.linear.x = vel.linear_velocity.x;
-    twist_msg.twist.linear.y = vel.linear_velocity.y;
-    twist_msg.twist.linear.z = vel.linear_velocity.z;
+    if (device_name == "tracker_1"){  
+      T_x_buffer.push_back(vel.linear_velocity.x);
+      T_y_buffer.push_back(vel.linear_velocity.y);
+      T_z_buffer.push_back(vel.linear_velocity.z);
+
+      float v_x = 0;
+      float v_y = 0;
+      float v_z = 0;
+
+      for(unsigned int i=0; i<T_x_buffer.size(); i++){
+        v_x += T_x_buffer[i];
+        v_y += T_y_buffer[i];
+        v_z += T_z_buffer[i];
+      }
+
+      twist_msg.twist.linear.x = v_x/(T_x_buffer.size());
+      twist_msg.twist.linear.y = v_z/(T_z_buffer.size());
+      twist_msg.twist.linear.z = v_y/(T_y_buffer.size());
+     // this->tracker_v_x = vel.linear_velocity.x;
+     // this->tracker_v_y = vel.linear_velocity.y;
+     // this->tracker_v_z = vel.linear_velocity.z;
+    }
+    else if (device_name == "controller_1"){
+      C_x_buffer.push_back(vel.linear_velocity.x);
+      C_y_buffer.push_back(vel.linear_velocity.y);
+      C_z_buffer.push_back(vel.linear_velocity.z);
+
+      float v_x = 0;
+      float v_y = 0;
+      float v_z = 0;
+
+      for(unsigned int i=0; i<C_x_buffer.size(); i++){
+        v_x += C_x_buffer[i];
+        v_y += C_y_buffer[i];
+        v_z += C_z_buffer[i];
+      }
+
+      twist_msg.twist.linear.x = v_x/(C_x_buffer.size());
+      twist_msg.twist.linear.y = v_z/(C_z_buffer.size());
+      twist_msg.twist.linear.z = v_y/(C_y_buffer.size());
+     // twist_msg.twist.linear.x = (vel.linear_velocity.x+this->controller_v_x)/2;
+     // twist_msg.twist.linear.y = (vel.linear_velocity.z+this->controller_v_z)/2;
+     // twist_msg.twist.linear.z = (vel.linear_velocity.y+this->controller_v_y)/2;
+     // this->controller_v_x = vel.linear_velocity.x;
+     // this->controller_v_y = vel.linear_velocity.y;
+     // this->controller_v_z = vel.linear_velocity.z;
+    }
+
     twist_msg.twist.angular.x = vel.angular_velocity.x;
-    twist_msg.twist.angular.y = vel.angular_velocity.y;
-    twist_msg.twist.angular.z = vel.angular_velocity.z;
+    twist_msg.twist.angular.y = vel.angular_velocity.z;
+    twist_msg.twist.angular.z = vel.angular_velocity.y;
 
     twist_msg.covariance = {{0.01, 0, 0, 0, 0, 0,  // covariance on pose x
                                 0, 0.01, 0, 0, 0, 0,  // covariance on pose y
@@ -287,7 +350,8 @@ nav_msgs::Odometry HtcViveTrackerAlgNode::CreateOdometryFromPoseVel(const geomet
     nav_msgs::Odometry odom;
     odom.header.stamp = ros::Time::now();
     odom.header.frame_id = this->alg_.WORLD_NAME; 
-    odom.child_frame_id = this->target_frame_name_;
+    //odom.child_frame_id = this->target_frame_name_;
+    odom.child_frame_id = device_name;
     odom.pose = pose_msg;
     odom.twist = twist_msg;
     return odom;