From 364024675d6f4989c93bdb32fa230b4e09d404b3 Mon Sep 17 00:00:00 2001
From: pmlab <lfreixas@iri.upc.edu>
Date: Tue, 9 Oct 2018 13:30:41 +0200
Subject: [PATCH] started implementing usage of robot_pose_ekf
---
include/htc_vive_tracker_alg_node.h | 5 +++++
launch/publish_wam_chaperone_link.launch | 1 +
launch/wam_follow_device.launch | 6 ++++++
src/htc_vive_tracker_alg_node.cpp | 26 ++++++++++++++++++++++++
4 files changed, 38 insertions(+)
diff --git a/include/htc_vive_tracker_alg_node.h b/include/htc_vive_tracker_alg_node.h
index 2524266..9004498 100644
--- a/include/htc_vive_tracker_alg_node.h
+++ b/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
diff --git a/launch/publish_wam_chaperone_link.launch b/launch/publish_wam_chaperone_link.launch
index 56f8518..1f607fe 100644
--- a/launch/publish_wam_chaperone_link.launch
+++ b/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>
diff --git a/launch/wam_follow_device.launch b/launch/wam_follow_device.launch
index 34adb2b..1a7648b 100644
--- a/launch/wam_follow_device.launch
+++ b/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>
diff --git a/src/htc_vive_tracker_alg_node.cpp b/src/htc_vive_tracker_alg_node.cpp
index 5d62db8..4ebf16d 100644
--- a/src/htc_vive_tracker_alg_node.cpp
+++ b/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[])
{
--
GitLab