diff --git a/include/htc_vive_tracker_alg_node.h b/include/htc_vive_tracker_alg_node.h index 70101a0a640f7597072bc53708b7e6fe0e9db1a3..df4e2d59c0d4b850c6e8ae0cf347f9ea7fd36f32 100644 --- a/include/htc_vive_tracker_alg_node.h +++ b/include/htc_vive_tracker_alg_node.h @@ -30,9 +30,11 @@ #include "iri_htc_vive_tracker/GetButtonPressed.h" #include "iri_htc_vive_tracker/TriggerHapticPulse.h" +#include "tf/transform_listener.h" + + // [publisher subscriber headers] #include <geometry_msgs/TransformStamped.h> -#include <tf/transform_broadcaster.h> // [service client headers] // [action server client headers] @@ -45,15 +47,14 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra { private: // [publisher attributes] - const std::string BASE_NAME = "iri_wam_link_base"; - const std::string WORLD_NAME = "chaperone"; - const std::string DEVICE_NOT_FOUND_MSG = "Device not found"; - geometry_msgs::TransformStamped transform_stamped_; //Transformation from base to world. In this case, WAM to CHAPERONE // [subscriber attributes] + ros::Publisher pose_publisher_; + tf::TransformListener tf_listener_; + // // [service attributes] // [client attributes] @@ -72,7 +73,11 @@ class HtcViveTrackerAlgNode : public algorithm_base::IriBaseAlgorithm<HtcViveTra uint32_t haptic_pulse_strength_; bool publish_hmd_; - + bool frame_names_set; + + std::string target_frame_name_; + std::string source_frame_name_; + ros::ServiceServer trigger_pulse_server_; bool trigger_pulse_serverCallback(iri_htc_vive_tracker::TriggerHapticPulse::Request &req, iri_htc_vive_tracker::TriggerHapticPulse::Response &res); diff --git a/launch/publish_wam_chaperone_link.launch b/launch/publish_wam_chaperone_link.launch index 2bb7abe2d6bd03b200831de1f4de18c8ceb2d1d7..e3b2086a7fddc18302ca93c0ae9aeed6299d5130 100644 --- a/launch/publish_wam_chaperone_link.launch +++ b/launch/publish_wam_chaperone_link.launch @@ -1,4 +1,6 @@ <launch> + <arg name="target_frame" default="tracker_1" /> + <!-- HTC Vive Transformation --> <!--static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms --> <node pkg="tf" @@ -18,5 +20,7 @@ <node pkg="iri_htc_vive_tracker" type = "iri_htc_vive_tracker" name = "iri_htc_vive_tracker" > + <param name="target_frame" type="string" value="$(arg target_frame)"/> + </node> </launch> diff --git a/launch/wam_follow_tracker.launch b/launch/wam_follow_tracker.launch index 578f868e818376bf0665181a50e38f6d72d209dc..f034004c5f0c466ad6a3d12adb736c448891574c 100644 --- a/launch/wam_follow_tracker.launch +++ b/launch/wam_follow_tracker.launch @@ -1,11 +1,9 @@ <launch> <!-- Initialize htc vive and publish transformation --> - <include file="$(find iri_htc_vive_tracker)/launch/publish_wam_chaperone_link.launch"/> + <include file="$(find iri_htc_vive_tracker)/launch/publish_wam_chaperone_link.launch"> + <!-- Initialize tf-to-pose node from chaperone to tracker1--> + <arg name="target_frame" value ="tracker_1"/> - <!-- Initialize tf-to-pose node from chaperone to tracker1--> - <!-- Also remaps topic to /iri_wam/pose_surface--> - <include file="$(find iri_tf_to_pose)/launch/htcvive.launch"> - <arg name="source_frame" value ="chaperone"/> - <arg name="target_frame" value ="tracker_1"/> </include> + <remap from="iri_htc_vive_tracker/new_pose" to="iri_wam/pose_surface" /> </launch> diff --git a/src/htc_vive_tracker_alg_node.cpp b/src/htc_vive_tracker_alg_node.cpp index 68b61496be7b3da3411b3f3b61855bc5ef11803e..f2fa5c76432909ab2a98edd5fdb69ab41a1740e0 100644 --- a/src/htc_vive_tracker_alg_node.cpp +++ b/src/htc_vive_tracker_alg_node.cpp @@ -22,6 +22,20 @@ HtcViveTrackerAlgNode::HtcViveTrackerAlgNode(void) : ROS_ERROR("Problem with initialization. Check other error messages"); } + this->frame_names_set = true; + if (this->public_node_handle_.hasParam("target_frame_name")){ + this->public_node_handle_.getParam("target_frame_name",this->target_frame_name_); + } + else { + ROS_INFO("No param set for target_frame"); + this->frame_names_set = false; + } + + this->source_frame_name_ = "chaperone"; + + pose_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("new_pose",100); + + // [init publishers] @@ -68,6 +82,25 @@ void HtcViveTrackerAlgNode::mainNodeThread(void) } } } + + if (this->frame_names_set){ + tf::StampedTransform stamped_transform; + try{ + bool is_transform_possible = this->tf_listener_.canTransform(this->source_frame_name_, this->target_frame_name_, ros::Time(0)); + 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); + this->pose_publisher_.publish(tf_pose); + } + else { + ROS_INFO ("Transform not possible"); + } + } + catch (tf::TransformException &ex) { + ROS_ERROR("%s",ex.what()); + ros::Duration(1.0).sleep(); + } + } // [fill msg structures] @@ -114,7 +147,7 @@ void HtcViveTrackerAlgNode::BroadcastPoseRotated(const std::string & device_name if (this->alg_.GetDevicePositionQuaternion(device_name,pose,quaternion)) { this->transform_stamped_.header.stamp = ros::Time::now(); - this->transform_stamped_.header.frame_id = WORLD_NAME; + 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]; @@ -171,7 +204,7 @@ tf::Quaternion HtcViveTrackerAlgNode::ApplyRotationForIRIStandardCoordinates(con bool HtcViveTrackerAlgNode::trigger_pulse_serverCallback(iri_htc_vive_tracker::TriggerHapticPulse::Request &req, iri_htc_vive_tracker::TriggerHapticPulse::Response &res) { res.success = this->alg_.TriggerHapticPulse(req.device_name, this->haptic_pulse_strength_); if (!res.success) { - res.message = DEVICE_NOT_FOUND_MSG; + res.message = this->alg_.DEVICE_NOT_FOUND_MSG; } return true; @@ -182,7 +215,7 @@ bool HtcViveTrackerAlgNode::get_button_serverCallback(iri_htc_vive_tracker::GetB vr::EVRButtonId button_pressed = this->alg_.GetPressedButton(req.device_name); res.button_pressed = (int) button_pressed; res.success = this->alg_.IsDeviceDetected (req.device_name); - if (!res.success) res.message = DEVICE_NOT_FOUND_MSG; + if (!res.success) res.message = this->alg_.DEVICE_NOT_FOUND_MSG; return true; } /* main function */