diff --git a/include/htc_vive_tracker_alg_node.h b/include/htc_vive_tracker_alg_node.h
index 30b6bbef1152a0c11e5d2d93b77d3f793ea10e26..566131516fd47df82184c766168ad7655901f857 100644
--- a/include/htc_vive_tracker_alg_node.h
+++ b/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);
};
diff --git a/src/htc_vive_tracker_alg_node.cpp b/src/htc_vive_tracker_alg_node.cpp
index 2cead2762f06a0cbbd9ad57dc7b7c7e8ffbcc0f7..4c56d75a5770a233f5f5b04193faa1a2bc588473 100644
--- a/src/htc_vive_tracker_alg_node.cpp
+++ b/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;