Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Devel/tangofly toni #54

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Devel/tangofly toni #54

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
36ed0ea
Add pose stamped output from vicon
ToniRV Jun 24, 2017
c7355d4
Remove header frame id renaming
ToniRV Jun 24, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 23 additions & 0 deletions src/ros_vrpn_client.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,7 @@ void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker);
struct TargetState {
geometry_msgs::TransformStamped measured_transform;
geometry_msgs::TransformStamped estimated_transform;
geometry_msgs::PoseStamped estimated_pose;
nav_msgs::Odometry estimated_odometry;
};

Expand Down Expand Up @@ -108,6 +109,8 @@ class Rigid_Body {
nh.advertise<geometry_msgs::TransformStamped>("raw_transform", 1);
estimated_target_transform_pub_ =
nh.advertise<geometry_msgs::TransformStamped>("estimated_transform", 1);
estimated_target_pose_pub_ =
nh.advertise<geometry_msgs::PoseStamped>("estimated_pose", 1);
estimated_target_odometry_pub_ =
nh.advertise<nav_msgs::Odometry>("estimated_odometry", 1);
// Connecting to the vprn device and creating an associated tracker.
Expand Down Expand Up @@ -139,6 +142,11 @@ class Rigid_Body {
estimated_target_transform_pub_.publish(target_state->estimated_transform);
}

void publish_estimated_pose(TargetState* target_state) {
br.sendTransform(target_state->estimated_transform);
estimated_target_pose_pub_.publish(target_state->estimated_pose);
}

// Publishes the estimated target state to the odometry message.
void publish_estimated_odometry(TargetState* target_state) {
estimated_target_odometry_pub_.publish(target_state->estimated_odometry);
Expand All @@ -154,6 +162,7 @@ class Rigid_Body {
// Publishers
ros::Publisher measured_target_transform_pub_;
ros::Publisher estimated_target_transform_pub_;
ros::Publisher estimated_target_pose_pub_;
ros::Publisher estimated_target_odometry_pub_;
tf::TransformBroadcaster br;
// Vprn object pointers
Expand Down Expand Up @@ -332,6 +341,19 @@ void VRPN_CALLBACK track_target(void*, const vrpn_TRACKERCB tracker) {
orientation_estimate_B_W,
target_state->estimated_transform.transform.rotation);

// Populate the estimated pose message. Published in main loop.
target_state->estimated_pose.header.stamp = timestamp;
target_state->estimated_pose.header.frame_id = coordinate_system_string;
target_state->estimated_pose.pose.position.x =
target_state->estimated_transform.transform.translation.x;
target_state->estimated_pose.pose.position.y =
target_state->estimated_transform.transform.translation.y;
target_state->estimated_pose.pose.position.z =
target_state->estimated_transform.transform.translation.z;
tf::quaternionEigenToMsg(
orientation_estimate_B_W,
target_state->estimated_pose.pose.orientation);

// Populate the estimated odometry message. Published in main loop.
target_state->estimated_odometry.header.stamp = timestamp;
target_state->estimated_odometry.header.frame_id = coordinate_system_string;
Expand Down Expand Up @@ -422,6 +444,7 @@ int main(int argc, char* argv[]) {
if (fresh_data == true) {
tool.publish_measured_transform(target_state);
tool.publish_estimated_transform(target_state);
tool.publish_estimated_pose(target_state);
tool.publish_estimated_odometry(target_state);
fresh_data = false;
}
Expand Down
12 changes: 12 additions & 0 deletions src/vicon_estimation_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@

#include <ros/ros.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
Expand All @@ -51,6 +52,8 @@ class ViconDataListener {
estimated_transform_pub_ =
nh_private.advertise<geometry_msgs::TransformStamped>(
"estimated_transform", 10);
estimated_pose_pub_ =
nh_private.advertise<geometry_msgs::PoseStamped>("estimated_pose", 10);
estimated_odometry_pub_ =
nh_private.advertise<nav_msgs::Odometry>("estimated_odometry", 10);
// Getting the object name
Expand Down Expand Up @@ -89,6 +92,13 @@ class ViconDataListener {
estimated_transform.transform.translation);
tf::quaternionEigenToMsg(orientation_estimate_B_W,
estimated_transform.transform.rotation);
// Creating estimated pose message
geometry_msgs::PoseStamped estimated_pose;
estimated_pose.header = msg->header;
tf::pointEigenToMsg(position_estimate_W,
estimated_pose.pose.position);
tf::quaternionEigenToMsg(orientation_estimate_B_W,
estimated_pose.pose.orientation);
// Creating estimated odometry message
nav_msgs::Odometry estimated_odometry;
estimated_odometry.header = msg->header;
Expand All @@ -103,6 +113,7 @@ class ViconDataListener {
estimated_odometry.twist.twist.angular);
// Publishing the estimates
estimated_transform_pub_.publish(estimated_transform);
estimated_pose_pub_.publish(estimated_pose);
estimated_odometry_pub_.publish(estimated_odometry);
// Publishing the estimator intermediate results
vicon_odometry_estimator_->publishIntermediateResults(msg->header.stamp);
Expand All @@ -113,6 +124,7 @@ class ViconDataListener {
ros::Subscriber raw_transform_sub_;
// Estimate publishers
ros::Publisher estimated_transform_pub_;
ros::Publisher estimated_pose_pub_;
ros::Publisher estimated_odometry_pub_;
// Name of the tracked object
std::string object_name_;
Expand Down