articulated_door_model.cpp

Go to the documentation of this file.

#include <ros/ros.h>

#include "tf/message_filter.h"
#include "message_filters/subscriber.h"
#include "tf/transform_listener.h"
#include <tf/transform_broadcaster.h>

#include "articulation_models/models/factory.h"

#include "articulation_msgs/ModelMsg.h"
#include "articulation_msgs/TrackMsg.h"
#include "articulation_msgs/ParamMsg.h"
#include "geometry_msgs/PoseStamped.h"

#include <sstream>
#include <cmath> //abs

#include <visualization_msgs/Marker.h>

using namespace articulation_models;
using namespace articulation_msgs;

//Global storage
ModelMsg global_model_msg;

//FUNCTIONS
visualization_msgs::Marker visualize_door_model(GenericModelPtr model);
void print(tf::Matrix3x3 m);
void print(tf::Quaternion q);
void print(tf::Vector3 v);
std::string stream(tf::Vector3 v);
std::string stream(tf::Quaternion v);
std::string stream(tf::StampedTransform tf);
void publish_door_coordinate_frame(GenericModelPtr generic_model);
tf::Transform compute_aligned_door_frame(GenericModelPtr generic_model);

void poseCallback(const tf::TransformListener* listener, 
                  std::string target_frame, 
                  geometry_msgs::PoseStamped::ConstPtr pose_stamped);

int main(int argc, char** argv) 
{
  ros::init(argc, argv, "articulated_door_model_fitting");
  ros::NodeHandle n;
  ros::Publisher model_pub = n.advertise<ModelMsg> ("door_model", 5);
  ros::Publisher model_visualization_pub = n.advertise<visualization_msgs::Marker> ("door_trajectory", 5);

  message_filters::Subscriber<geometry_msgs::PoseStamped> pose_sub;
  pose_sub.subscribe(n, "pose0", 10);
  std::string fixed_frame;
  ros::NodeHandle nh("~");
  nh.param<std::string>("fixed_frame", fixed_frame, "/odom_combined");
  ROS_DEBUG("Using %s as fixed frame", fixed_frame.c_str());
  tf::TransformListener tf_listener;
  tf::MessageFilter<geometry_msgs::PoseStamped> tf_filter(pose_sub, tf_listener, fixed_frame, 10);
  tf_filter.registerCallback(boost::bind(&poseCallback, &tf_listener, fixed_frame, _1));

  tf::TransformBroadcaster br;
  ros::Rate loop_rate(10);

  MultiModelFactory factory;
  global_model_msg.name = "rotational";
  ParamMsg sigma_param;
  sigma_param.name = "sigma_position";
  sigma_param.value = 0.02;
  sigma_param.type = ParamMsg::PRIOR;
  global_model_msg.params.push_back(sigma_param);
  unsigned long int prev_size = 0;
  GenericModelPtr model_instance;
  while (ros::ok()) {
    if(global_model_msg.track.pose.size() < 4)
      ROS_INFO_THROTTLE(2, "Not enough samples: %lu", global_model_msg.track.pose.size());
    else {
      //New observation? Refit model
      if(prev_size < global_model_msg.track.pose.size()) 
      {
        model_instance = factory.restoreModel(global_model_msg);
        model_instance->fitModel();
        model_instance->evaluateModel();
        ROS_INFO_STREAM_THROTTLE(5,"log LH of rotational model: " << model_instance->getLogLikelihood(false));
      }

      //Send model to the world
      ROS_FATAL_COND(model_instance.get() == NULL, "Used model_instance before restoring the model?"); //since prev_size is initialized with 0
      ModelMsg fitted_model_msg = model_instance->getModel();
      tf::Transform aligned_door_tf = compute_aligned_door_frame(model_instance);
      tf::StampedTransform door_stamped(aligned_door_tf, ros::Time::now(), fitted_model_msg.track.header.frame_id, "/door");
      br.sendTransform(door_stamped);
      ROS_INFO_STREAM_THROTTLE(1, "<node pkg=\"tf\" type=\"static_transform_publisher\" name=\"door_broadcaster\" args=\"" << stream(door_stamped)<<" 100\" />");

      if(model_pub.getNumSubscribers() > 0)
        model_pub.publish(fitted_model_msg);
      if(model_visualization_pub.getNumSubscribers() > 0)
        model_visualization_pub.publish(visualize_door_model(model_instance));

    }
    ros::spinOnce();
    loop_rate.sleep();
  }
}

void poseCallback(const tf::TransformListener* listener, 
                  std::string target_frame, 
                  geometry_msgs::PoseStamped::ConstPtr pose_stamped)
{
    try 
    {
      geometry_msgs::PoseStamped pose_in_fixed_frame;
      listener->transformPose(target_frame, *pose_stamped, pose_in_fixed_frame);
      
      //ROS_INFO_STREAM_ONCE("Storing the first pose:\n " << pose_in_fixed_frame);
      global_model_msg.track.pose.push_back(pose_in_fixed_frame.pose);
      global_model_msg.track.header = pose_in_fixed_frame.header; //update header, it's used elsewhere
      //ROS_INFO_STREAM(pose_in_fixed_frame);
      ROS_INFO("Recorded %lu. pose", global_model_msg.track.pose.size());
    }
    catch (tf::TransformException &ex) 
    {
      ROS_ERROR("Failure %s\n", ex.what()); //Print exception which was caught
    }
}


visualization_msgs::Marker visualize_door_model(GenericModelPtr generic_model)
{
  ModelMsg model = generic_model->getModel();
  visualization_msgs::Marker trajectory;
  trajectory.header = model.track.header;
  trajectory.id = 1;
  trajectory.type = visualization_msgs::Marker::POINTS;
  trajectory.action = visualization_msgs::Marker::ADD;
  trajectory.pose.orientation.w = 1.0;
  trajectory.scale.x = 0.02;
  trajectory.scale.y = 0.02;
  trajectory.scale.z = 0.02;
  trajectory.color.a = 0.5;
  trajectory.color.r = 0.5;
  trajectory.color.g = 0.5;

  for(unsigned int i = 0; i < model.track.pose.size(); i++){
    trajectory.points.push_back(model.track.pose[i].position);
  }
  geometry_msgs::Point center;
  center.x = generic_model->getParam("rot_center.x");
  center.y = generic_model->getParam("rot_center.y");
  center.z = generic_model->getParam("rot_center.z");
  trajectory.points.push_back(center);
  return trajectory;
}


tf::Transform compute_aligned_door_frame(GenericModelPtr generic_model) 
{
  //static tf::TransformBroadcaster br;
  TrackMsg tm = global_model_msg.track;
  //GET DOOR MODEL PARAMETER
  double q_min = generic_model->getParam("q_min[0]");
  double q_max = generic_model->getParam("q_max[0]");
  double rot_radius = generic_model->getParam("rot_radius");
  tf::Vector3 rot_center(generic_model->getParam("rot_center.x"),
                         generic_model->getParam("rot_center.y"),
                         generic_model->getParam("rot_center.z"));
        
  tf::Quaternion rot_axis(generic_model->getParam("rot_axis.x"),
                          generic_model->getParam("rot_axis.y"),
                          generic_model->getParam("rot_axis.z"),
                          generic_model->getParam("rot_axis.w"));
  
  tf::Quaternion rot_orientation(generic_model->getParam("rot_orientation.x"),
                                 generic_model->getParam("rot_orientation.y"),
                                 generic_model->getParam("rot_orientation.z"),
                                 generic_model->getParam("rot_orientation.w"));

  //COMPUTE POSES FOR AXIS FRAME, Q_MIN, Q_MAX
  tf::Transform center(rot_axis,rot_center);
  tf::Transform rotZ_min(tf::Quaternion(tf::Vector3(0, 0, 1), -q_min), tf::Vector3(0, 0, 0));
  tf::Transform rotZ_max(tf::Quaternion(tf::Vector3(0, 0, 1), -q_max), tf::Vector3(0, 0, 0));
  tf::Transform r(tf::Quaternion(0,0,0,1), tf::Vector3(rot_radius, 0, 0));
  tf::Transform offset(rot_orientation, tf::Vector3(0, 0, 0));
  tf::Transform qmin_pose = rotZ_min * r * offset;
  tf::Transform qmax_pose = rotZ_max * r * offset;

/*
  br.sendTransform(tf::StampedTransform(center, tm.header.stamp, tm.header.frame_id, "a_door"));
  br.sendTransform(tf::StampedTransform(qmin_pose, tm.header.stamp, "a_door", "qmin"));
  br.sendTransform(tf::StampedTransform(qmax_pose, tm.header.stamp, "a_door", "qmax"));

  tf::Transform closed_door_cb(tf::Quaternion(0,0,0,1), tf::Vector3(1,0,0));
  br.sendTransform(tf::StampedTransform(closed_door_cb, 
                                        tm.header.stamp, 
                                        "aligned_door", 
                                        "closed"));

  tf::Transform open_door_cb(tf::Quaternion(0,0,0,1), tf::Vector3(0,1,0));
  br.sendTransform(tf::StampedTransform(open_door_cb, 
                                        tm.header.stamp, 
                                        "aligned_door", 
                                        "open"));
                                        */

  //COMPUTE POSE FOR FIRST POSE RECORDED
  tf::Pose q1st_pose_in_fixed_frame;
  tf::poseMsgToTF(tm.pose[0], q1st_pose_in_fixed_frame);
  tf::Transform q1st_pose = center.inverse() * q1st_pose_in_fixed_frame;
  //br.sendTransform(tf::StampedTransform(q1st_pose, tm.header.stamp, "door", "q1st"));

  tf::Vector3 q1st_position = q1st_pose.getOrigin();
  tf::Vector3 qmin_position = qmin_pose.getOrigin();
  tf::Vector3 qmax_position = qmax_pose.getOrigin();
  tf::Vector3 closed_door_handle_in_ff, open_door_handle_in_ff;
  //FIND OUT WHETHER Q_MIN OR Q_MAX ARE NEARER TO Q_START
  //Assuming qmax_ and qmin_position are of same (radius) length
  ROS_ERROR_COND((abs(qmin_position.length2() - qmax_position.length2()) > 0.001), "Reprojected poses should have same length");
  if(q1st_position.dot(qmax_position) > q1st_position.dot(qmin_position))
  {
    //First pose recorded is more similar to the pose at q_max
    closed_door_handle_in_ff = center*qmax_position;
    open_door_handle_in_ff   = center*qmin_position;
  } else {
    closed_door_handle_in_ff = center*qmin_position;
    open_door_handle_in_ff   = center*qmax_position;
  }
  /*DEBUG
  tf::Transform cdhiff(tf::Quaternion(0,0,0,1), closed_door_handle_in_ff);
  br.sendTransform(tf::StampedTransform(cdhiff, tm.header.stamp, tm.header.frame_id, "a_closed_door_handle"));
  tf::Transform odhiff(tf::Quaternion(0,0,0,1), open_door_handle_in_ff);
  br.sendTransform(tf::StampedTransform(odhiff, tm.header.stamp, tm.header.frame_id, "a_open_door_handle"));
  //END DEBUG*/

  tf::Vector3 closed_axis = closed_door_handle_in_ff - center.getOrigin();
  tf::Vector3 open_axis = open_door_handle_in_ff - center.getOrigin();
  /*DEBUG
  tf::Transform a(tf::Quaternion(0,0,0,1), open_axis);
  br.sendTransform(tf::StampedTransform(a, tm.header.stamp, tm.header.frame_id, "a_open_axis_in_ff"));
  tf::Transform b(tf::Quaternion(0,0,0,1), closed_axis);
  br.sendTransform(tf::StampedTransform(b, tm.header.stamp, tm.header.frame_id, "a_closed_axis_in_ff"));
  ROS_WARN("Line: %d", __LINE__);
  //END DEBUG*/

  tf::Vector3 rotation_axis = closed_axis.cross(open_axis);
  open_axis = rotation_axis.cross(closed_axis);
  closed_axis = closed_axis.normalize();
  print(closed_axis);
  open_axis = open_axis.normalize();
  print(open_axis);
  rotation_axis = rotation_axis.normalize();
  print(rotation_axis);

  tf::Matrix3x3 basis(closed_axis.x(), open_axis.x(), rotation_axis.x(),
                      closed_axis.y(), open_axis.y(), rotation_axis.y(),
                      closed_axis.z(), open_axis.z(), rotation_axis.z());
  print(basis);
  tf::Quaternion qbasis; basis.getRotation(qbasis);
  print(qbasis);
  return tf::Transform(basis, center.getOrigin());
  /*DEBUG
  tf::Transform aligned_cdhiff(tf::Quaternion(0,0,0,1), tf::Vector3(1,0,0));
  br.sendTransform(tf::StampedTransform(aligned_cdhiff, tm.header.stamp, "aligned_door", "aligned_closed_door_handle"));
  ROS_WARN("Line: %d", __LINE__);
  tf::Transform aligned_odhiff(tf::Quaternion(0,0,0,1), tf::Vector3(0,1,0));
  br.sendTransform(tf::StampedTransform(aligned_odhiff, tm.header.stamp, "aligned_door", "aligned_open_door_handle"));
  ROS_WARN("Line: %d", __LINE__);
  //END DEBUG*/
}

void print(tf::Matrix3x3 m){
  ROS_DEBUG("RotM:\n%.5f, %.5f, %.5f\n%.5f, %.5f, %.5f\n%.5f, %.5f, %.5f\n", 
            m[0][0], m[0][1], m[0][2], 
            m[1][0], m[1][1], m[1][2], 
            m[2][0], m[2][1], m[2][2]);
}
void print(tf::Quaternion q){
  ROS_DEBUG("Quat: %.4f, %.4f, %.4f, %.4f", q.getX(), q.getY(), q.getZ(), q.w());
}
void print(tf::Vector3 v){
  ROS_DEBUG("Vec: %.4f, %.4f, %.4f, %.4f", v.getX(), v.getY(), v.getZ(), v.w());
}
std::string stream(tf::Vector3 v){
  std::stringstream ss;
  ss << v.x() << " "  << v.y() << " "  << v.z(); 
  return ss.str();
}
std::string stream(tf::Quaternion v){
  std::stringstream ss;
  ss << v.x() << " "  << v.y() << " "  << v.z()<< " "  << v.w();
  return ss.str();
}
std::string stream(tf::StampedTransform tf){
  std::stringstream ss;
  ss << stream(tf.getOrigin()) << " " <<  stream(tf.getRotation()) << " " << tf.frame_id_ << " " << tf.child_frame_id_;
  return ss.str();
}