diff --git a/include/htc_vive_tracker_alg.h b/include/htc_vive_tracker_alg.h
index 5a922b7a0c5f4bd30eaa40e847d019d7441546fa..eb5dd3d7d2585df9642b65906a8d79a73b7670b8 100644
--- a/include/htc_vive_tracker_alg.h
+++ b/include/htc_vive_tracker_alg.h
@@ -145,6 +145,7 @@ class HtcViveTrackerAlgorithm
bool TriggerHapticPulse(const std::string & device_name, uint32_t strength);
+ //TODO decide if this functions will return orientation as given or always the same
geometry_msgs::PoseStamped PoseFromTF(const tf::StampedTransform & stamped_transform);
// the driver parameters
diff --git a/include/htc_vive_tracker_alg_node.h b/include/htc_vive_tracker_alg_node.h
index 252426603363c289968dfd56769bbe4a3ce64215..1416fcb493c770543f8241899e32d3cc3180fc4b 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]
@@ -52,7 +53,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 +150,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/htc_vive_main.launch b/launch/htc_vive_main.launch
new file mode 100644
index 0000000000000000000000000000000000000000..282f75106e56f490f06bf6ad30e5832c813acd9a
--- /dev/null
+++ b/launch/htc_vive_main.launch
@@ -0,0 +1,9 @@
+<launch>
+ <arg name="target_frame" default="tracker_1" />
+ <node pkg="iri_htc_vive_tracker"
+ type = "iri_htc_vive_tracker"
+ name = "iri_htc_vive_tracker"
+ output = "screen">
+ <param name="~target_frame_name" type="string" value="$(arg target_frame)"/>
+ </node>
+</launch>
diff --git a/launch/publish_wam_chaperone_link.launch b/launch/publish_wam_chaperone_link.launch
index 56f85185014cb52bff609c101c038f38affbc2d5..b09a6209233fc69195bc180bdd70cbc0c7334780 100644
--- a/launch/publish_wam_chaperone_link.launch
+++ b/launch/publish_wam_chaperone_link.launch
@@ -22,7 +22,7 @@
name = "iri_htc_vive_tracker"
output = "screen">
<param name="~target_frame_name" type="string" value="$(arg target_frame)"/>
- <remap from="~new_pose" to="/iri_wam/pose_surface"/>
</node>
+
</launch>
diff --git a/launch/wam_follow_device.launch b/launch/wam_follow_device.launch
index 34adb2b7d263923fc2725007106aa312d6460939..48e066dd4521fb569d3e4665bb6f013f43e86471 100644
--- a/launch/wam_follow_device.launch
+++ b/launch/wam_follow_device.launch
@@ -5,4 +5,11 @@
<!-- Initialize tf-to-pose node from chaperone to tracker1-->
<arg name="target_frame" value ="$(arg device)"/>
</include>
+
+
+ <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
+ <rosparam command="load" file="$(find iri_htc_vive_tracker)/params/ekf_htcvive.yaml" />
+ <remap from="odometry/filtered" to="filtered_odometry"/>
+ </node>
+
</launch>
diff --git a/params/ekf_htcvive.yaml b/params/ekf_htcvive.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..74e0d101d639d5889e6a3e9e71170e3b2d44827e
--- /dev/null
+++ b/params/ekf_htcvive.yaml
@@ -0,0 +1,251 @@
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of the inputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+frequency: 30
+
+# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+# filter will generate new output. Defaults to 1 / frequency if not specified.
+sensor_timeout: 0.1
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+two_d_mode: false
+
+# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+# unspecified.
+transform_time_offset: 0.0
+
+# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
+# Defaults to 0.0 if unspecified.
+transform_timeout: 0.0
+
+# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+# unhappy with any settings or data.
+print_diagnostics: true
+
+# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+# effects on the performance of the node. Defaults to false if unspecified.
+debug: false
+
+# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
+debug_out_file: /path/to/debug/file.txt
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+publish_tf: true
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+publish_acceleration: false
+
+# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+# Here is how to use the following settings:
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+# odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+# from landmark observations) then:
+# 3a. Set your "world_frame" to your map_frame value
+# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+# estimation node from robot_localization! However, that instance should *not* fuse the global data.
+map_frame: map # Defaults to "map" if unspecified
+odom_frame: odom # Defaults to "odom" if unspecified
+base_link_frame: iri_wam_link_base # Defaults to "base_link" if unspecified
+world_frame: odom # Defaults to the value of odom_frame if unspecified
+
+# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+# default values, and must be specified.
+odom0: iri_htc_vive_tracker/vo
+
+# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+# if unspecified, effectively making this parameter required for each sensor.
+odom0_config: [true, true, true,
+ true, true, true,
+ true, true, true,
+ true, true, true,
+ true, true, true]
+
+# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
+# the size of the subscription queue so that more measurements are fused.
+odom0_queue_size: 2
+
+# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
+# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
+# algorithm.
+odom0_nodelay: false
+
+# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+# for twist measurements has no effect.
+odom0_differential: false
+
+# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
+# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
+# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
+# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
+odom0_relative: false
+
+# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
+# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
+# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
+# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
+# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
+# the thresholds.
+odom0_pose_rejection_threshold: 5
+odom0_twist_rejection_threshold: 1
+
+# Further input parameter examples
+odom1: example/another_odom
+odom1_config: [false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false]
+odom1_differential: false
+odom1_relative: true
+odom1_queue_size: 2
+odom1_pose_rejection_threshold: 2
+odom1_twist_rejection_threshold: 0.2
+odom1_nodelay: false
+
+pose0: example/pose
+pose0_config: [false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false]
+pose0_differential: true
+pose0_relative: false
+pose0_queue_size: 5
+pose0_rejection_threshold: 2 # Note the difference in parameter name
+pose0_nodelay: false
+
+twist0: example/twist
+twist0_config: [false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false]
+twist0_queue_size: 3
+twist0_rejection_threshold: 2
+twist0_nodelay: false
+
+imu0: example/imu
+imu0_config: [false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false,
+ false, false, false]
+imu0_nodelay: false
+imu0_differential: false
+imu0_relative: true
+imu0_queue_size: 5
+imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
+imu0_twist_rejection_threshold: 0.8 #
+imu0_linear_acceleration_rejection_threshold: 0.8 #
+
+# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+imu0_remove_gravitational_acceleration: true
+
+# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
+# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
+# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
+# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
+# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
+# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
+# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
+# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
+# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
+# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
+# inputs, the control term will be ignored.
+# Whether or not we use the control input during predicition. Defaults to false.
+use_control: true
+# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+# false.
+stamped_control: false
+# The last issued control command will be used in prediction for this period. Defaults to 0.2.
+control_timeout: 0.2
+# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+control_config: [true, false, false, false, false, true]
+# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+# Acceleration and deceleration limits are not always the same for robots.
+deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+# gains
+acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+# gains
+deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+
+# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+# However, if users find that a given variable is slow to converge, one approach is to increase the
+# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+# unspecified.
+process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
+
+# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+# question. Users should take care not to use large values for variables that will not be measured directly. The values
+# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
+#if unspecified.
+initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]
+
diff --git a/src/htc_vive_tracker_alg_node.cpp b/src/htc_vive_tracker_alg_node.cpp
index 5d62db8ec5062165983d999513992745dc313cde..8960e9b23922ac20b3daaeea0c05c64d14dc4469 100644
--- a/src/htc_vive_tracker_alg_node.cpp
+++ b/src/htc_vive_tracker_alg_node.cpp
@@ -33,7 +33,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,7 +89,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);
- this->pose_publisher_.publish(tf_pose);
+ 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);
}
else {
ROS_INFO ("Transform not possible");
@@ -225,6 +227,42 @@ 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;
+
+ 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
+ 0, 0, 0.01, 0, 0, 0, // covariance on pose z
+ 0, 0, 0, 1, 0, 0, // covariance on rot x
+ 0, 0, 0, 0, 1, 0, // covariance on rot y
+ 0, 0, 0, 0, 0, 1}; // covariance on rot z
+
+ geometry_msgs::PoseWithCovariance pose_msg;
+ pose_msg.pose = pose.pose;
+ pose_msg.covariance = {0.01, 0, 0, 0, 0, 0, // covariance on pose x
+ 0, 0.01, 0, 0, 0, 0, // covariance on pose y
+ 0, 0, 0.01, 0, 0, 0, // covariance on pose z
+ 0, 0, 0, 1, 0, 0, // covariance on rot x
+ 0, 0, 0, 0, 1, 0, // covariance on rot y
+ 0, 0, 0, 0, 0, 1}; // covariance on rot z
+
+ 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[])
{