teleop_microscribe.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2009, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <string>

#include <ros/ros.h>

#include <geometry_msgs/PoseStamped.h>
#include <std_srvs/Empty.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

std::string master_root, master_tip;
std::string slave_root, slave_tip;
double scaling = 1.0;

bool tracking = false;
tf::Transform offset;
tf::Transform root_offset;
tf::Transform tip_offset;
tf::Vector3 trans_offset;
tf::Quaternion rot_offset;
tf::TransformListener *g_tf = NULL;

bool engage(std_srvs::Empty::Request &req,
            std_srvs::Empty::Response &resp)
{
  assert(g_tf);
  tracking = false;

  tf::StampedTransform master_pose, slave_pose;
  g_tf->lookupTransform(master_root, master_tip, ros::Time(0), master_pose);
  g_tf->lookupTransform(slave_root, slave_tip, ros::Time(0), slave_pose);

  root_offset = slave_pose * master_pose.inverse();
  root_offset.setRotation(tf::Quaternion(0,0,0,1));

  tip_offset = slave_pose * master_pose.inverse();
  tip_offset.setOrigin(tf::Vector3(0,0,0));

  offset = slave_pose * master_pose.inverse();

  trans_offset = slave_pose.getOrigin() - scaling * master_pose.getOrigin();
  rot_offset = slave_pose.getRotation() * master_pose.getRotation().inverse();
  rot_offset = master_pose.getRotation().inverse() * slave_pose.getRotation();
  rot_offset.setEuler(0,0,0);

  ROS_INFO("Engaged");

  tracking = true;
  return true;
}

bool disengage(std_srvs::Empty::Request &req,
               std_srvs::Empty::Response &resp)
{
  tracking = false;
  ROS_INFO("Disengaged");
  return true;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "teleop_microscribe");

  ros::NodeHandle pn("~");
  ros::NodeHandle node;

  pn.param("master_root", master_root, std::string("XX/r_ms/base_link"));
  pn.param("master_tip", master_tip, std::string("XX/r_ms/stylus_tip_link"));
  pn.param("slave_root", slave_root, std::string("XX/base_link"));
  pn.param("slave_tip", slave_tip, std::string("XX/r_gripper_tool_frame"));
  pn.param("scaling", scaling, 1.0);

  tf::TransformListener tf;
  g_tf = &tf;
  tf::TransformBroadcaster tf_broadcaster;

  ros::Publisher pub_command = node.advertise<geometry_msgs::PoseStamped>("command", 3);
  //ros::Publisher pub_command = node.advertise<geometry_msgs::PoseStamped>("/r_cart/command_pose", 3);

  ros::ServiceServer srv_engage = pn.advertiseService("engage", engage);
  ros::ServiceServer srv_disengage = pn.advertiseService("disengage", disengage);

  //tracking = true;
  ros::Rate rate(100);
  while (ros::ok())
  {
    ros::spinOnce();
    if (tracking)
    {
      try
      {
        // Determines the pose of the master
        tf::Stamped<tf::Pose> master_pose;
        master_pose.frame_id_ = master_tip;
        master_pose.setIdentity();
        tf.transformPose(master_root, master_pose, master_pose);

        //printf("Master: %6.3f, %6.3f, %6.3f\n",
        //       master_pose.getOrigin().x(), master_pose.getOrigin().y(), master_pose.getOrigin().z());

        // Computes the desired slave pose
        tf::Stamped<tf::Pose> slave_desi;
        //(tf::Pose&)slave_desi = root_offset * master_pose * tip_offset.inverse();

        slave_desi.setOrigin(master_pose.getOrigin() * scaling + trans_offset);
        //slave_desi.setRotation(tf::Quaternion(0,0,0,1));
        //slave_desi.setRotation(rot_offset * master_pose.getRotation());
        slave_desi.setRotation(master_pose.getRotation() * rot_offset);
        slave_desi.frame_id_ = slave_root;
        slave_desi.stamp_ = ros::Time::now();

        // Publishes the desired slave pose
        geometry_msgs::PoseStamped slave_desi_msg;
        tf::poseStampedTFToMsg(slave_desi, slave_desi_msg);
        pub_command.publish(slave_desi_msg);

      }
      catch(const tf::TransformException &ex)
      {
        printf("Transform exception: %s\n", ex.what());
      }
    }

    // Publishes the fake transform between the master and the robot
    tf::StampedTransform master_in_slave;
    //(tf::Transform&)master_in_slave = root_offset;
    master_in_slave.setOrigin(trans_offset);
    master_in_slave.setRotation(tf::Quaternion(0,0,0,1));
    master_in_slave.stamp_ = ros::Time::now();
    master_in_slave.frame_id_ = slave_root;
    master_in_slave.child_frame_id_ = master_root;
    tf_broadcaster.sendTransform(master_in_slave);

    rate.sleep();
  }

  return 0;
}