pose.cpp

Go to the documentation of this file.

#include "common/pose.h"

slam::Pose::Pose(){}

void slam::Pose::advertisePoseMsg(ros::NodeHandle nh)
{
  // Advertise the pose publication
  pose_pub_ = nh.advertise<nav_msgs::Odometry>("corrected_odom", 1);
  graph_pub_ = nh.advertise<nav_msgs::Odometry>("graph", 1);
}

tf::Transform slam::Pose::correctOdom( tf::Transform current_odom,
                                              tf::Transform last_graph_pose,
                                              tf::Transform last_graph_odom)
{
  // Odometry difference
  tf::Transform odom_diff = last_graph_odom.inverse() * current_odom;

  // Compute the corrected pose
  return last_graph_pose * odom_diff;
}

void slam::Pose::publish(nav_msgs::Odometry odom_msg, tf::Transform pose, bool publish_graph)
{
  // Publish pose
  if (pose_pub_.getNumSubscribers() > 0)
  {
    nav_msgs::Odometry pose_msg = odom_msg;
    pose_msg.header.stamp = odom_msg.header.stamp;
    pose_msg.header.frame_id = params_.pose_frame_id;
    pose_msg.child_frame_id = params_.pose_child_frame_id;
    tf::poseTFToMsg(pose, pose_msg.pose.pose);
    pose_pub_.publish(pose_msg);
  }
  if (graph_pub_.getNumSubscribers() > 0 and publish_graph)
  {
    nav_msgs::Odometry pose_msg = odom_msg;
    pose_msg.header.stamp = odom_msg.header.stamp;
    pose_msg.header.frame_id = params_.pose_frame_id;
    pose_msg.child_frame_id = params_.pose_child_frame_id;
    tf::poseTFToMsg(pose, pose_msg.pose.pose);
    graph_pub_.publish(pose_msg);
  }
}