started implementing usage of robot_pose_ekf

include/htc_vive_tracker_alg_node.h

@@ -35,6 +35,7 @@
 
 // [publisher subscriber headers]
 #include <geometry_msgs/TransformStamped.h>
+#include <nav_msgs/Odometry.h>
 // [service client headers]
 
 // [action server client headers]
@@ -53,6 +54,7 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
     // [subscriber attributes]
 	
     ros::Publisher pose_publisher_;
+    ros::Publisher vo_publisher_;
     tf::TransformListener tf_listener_;
         //
     // [service attributes]
@@ -149,6 +151,9 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra
     void PrintAllDeviceNames();
     
     tf::Quaternion ApplyRotationForIRIStandardCoordinates(const tf::Quaternion & orig);
+    
+    nav_msgs::Odometry CreateOdometryFromPoseVel(const geometry_msgs::PoseStamped & pose, const Velocity & vel);
+    
 };
 
 #endif

launch/publish_wam_chaperone_link.launch

@@ -23,6 +23,7 @@
         output = "screen">
          <param name="~target_frame_name" type="string" value="$(arg target_frame)"/>
          <remap from="~new_pose" to="/iri_wam/pose_surface"/>
+         <remap from="~vo" to="/vo"/>
 
 	</node>
 </launch>

launch/wam_follow_device.launch

@@ -5,4 +5,10 @@
 	    <!-- Initialize tf-to-pose node from chaperone to tracker1-->
     	<arg name="target_frame" value ="$(arg device)"/>
 	</include>
+    
+	<include file="$(find robot_pose_ekf)/robot_pose_ekf.launch"/>
+    <param name="robot_pose_ekf/odom_used" value="false"/>
+    <param name="robot_pose_ekf/imu_used" value="false"/>
+    <param name="robot_pose_ekf/vo_used" value="false"/>
+
 </launch>

src/htc_vive_tracker_alg_node.cpp

@@ -34,6 +34,7 @@ HtcViveTrackerAlgNode::HtcViveTrackerAlgNode(void) :
     this->source_frame_name_ = "chaperone";
 
     pose_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("new_pose",100);
+    vo_publisher_ = this->public_node_handle_.advertise<nav_msgs::Odometry>("vo",100);
 
 
   // [init publishers]
@@ -89,6 +90,9 @@ void HtcViveTrackerAlgNode::PublishPoseOfDeviceToFollow(void){
         if (is_transform_possible) {
             this->tf_listener_.lookupTransform(this->source_frame_name_, this->target_frame_name_, ros::Time(0), stamped_transform);
             geometry_msgs::PoseStamped tf_pose = this->alg_.PoseFromTF(stamped_transform);
+            Velocity device_vel = this->alg_.GetDeviceVelocity(this->target_frame_name_);
+            nav_msgs::Odometry current_vo = this->CreateOdometryFromPoseVel(tf_pose, device_vel);
+            this->vo_publisher_.publish(current_vo);
             this->pose_publisher_.publish(tf_pose);
         }
         else {
@@ -225,6 +229,28 @@ bool HtcViveTrackerAlgNode::get_button_serverCallback(iri_htc_vive_tracker::GetB
 	if (!res.success) res.message = this->alg_.DEVICE_NOT_FOUND_MSG;
 	return true;
 }
+nav_msgs::Odometry HtcViveTrackerAlgNode::CreateOdometryFromPoseVel(const geometry_msgs::PoseStamped & pose, const Velocity & vel){
+    
+    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;
+    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;
+    //TODO Fill covariance
+    geometry_msgs::PoseWithCovariance pose_msg;
+    pose_msg.pose = pose.pose;
+    //TODO Fill covariance
+
+    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.pose = pose_msg;
+    odom.twist = twist_msg;
+    return odom;
+}
 /* main function */
 int main(int argc,char *argv[])
 {