perception stop command

#include <std_msgs/Bool.h>

// Initialize the ROS topic for recieving a stop command
std::size_t queue_size = 10;                                                                                                                                                                                                                                                      
stop_perception_sub_ = nh_.subscribe("/perception/stop_perception", queue_size, &Manipulator::stopPerceptionCallback, this);                                                                                                                                                      


void Manipulator::stopPerceptionCallback(const std_msgs::Bool::ConstPtr& msg)
{
  // The robot has finished moving the camera around
  // Send the result of the ROS Action request now
}

/**
 * \brief The robot has finished moving the camera around
 * \param bool result - ignore this value
 */
void stopPerceptionCallback(const std_msgs::Bool::ConstPtr& msg);                                                                                                                                                                                                                 

ros::Subscriber stop_perception_sub_;




========================================================
HEADER

#include <tf/transform_listener.h>

// Gets transforms between robot links
tf::TransformListener transform_listener_;                                                                                                                                                                                                                                        

/**
 * \brief Get the latest location of the camera on the robot from ROS
 * \param world_to_camera 4x4 matrix to fill in with transpose
 * \param time_stamp - the time that the arm was in this location
 * \return true on success  
 */
bool getCameraPose(Eigen::Affine3d& world_to_camera, ros::Time& time_stamp);                                                                                                                                                                                                       

/**
 * \brief Convert a tf transform to an eigen transform.
 *        Copied from https://github.com/ros/geometry/blob/indigo-devel/tf_conversions/src/tf_eigen.cpp#L73
 *        To reduce dependencies
 */
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e);                                                                                                                                                                                                              

FILE

bool Manipulator::getCameraPose(Eigen::Affine3d& world_to_camera, ros::Time& time_stamp)
{
  tf::StampedTransform camera_transform;                                                                                                                                                                                                                                          
  static const std::string parent_frame = "/world";                                                                                                                                                                                                                               
  static const std::string camera_frame = "/xtion_camera";                                                                                                                                                                                                                        

  try
  {
    // Wait to make sure a transform message has arrived
    tf_->waitForTransform(parent_frame, camera_frame, ros::Time(0), ros::Duration(1));                                                                                                                                                                                            
    // Get latest transform available
    tf_->lookupTransform(parent_frame, camera_frame, ros::Time(0), camera_transform);                                                                                                                                                                                             
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR_STREAM_NAMED("ddtr_perception", "Error: " << ex.what());                                                                                                                                                                                                            
    return false;                                                                                                                                                                                                                                                                 
  }

  // Copy results
  transformTFToEigen(camera_transform, world_to_camera);                                                                                                                                                                                                                          
  time_stamp = camera_transform.stamp_;                                                                                                                                                                                                                                           
  return true;
}

void Manipulator::transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
{
  for(int i=0; i<3; i++)                                                                                                                                                                                                                                                          
  {
    e.matrix()(i,3) = t.getOrigin()[i];                                                                                                                                                                                                                                           
    for(int j=0; j<3; j++)                                                                                                                                                                                                                                                        
    {
      e.matrix()(i,j) = t.getBasis()[i][j];                                                                                                                                                                                                                                       
    }
  }
  // Fill in identity in last row
  for (int col = 0 ; col < 3; col ++)                                                                                                                                                                                                                                             
    e.matrix()(3, col) = 0;                                                                                                                                                                                                                                                       
  e.matrix()(3,3) = 1;                                                                                                                                                                                                                                                            
};