RivetingController

/******************************************************************************
 * \file
 *
 * Copyright (C) Brno University of Technology (BUT)
 *
 * This file is part of software developed by Robo@FIT group.
 *
 * Author: Zdenek Materna (imaterna@fit.vutbr.cz)
 * Supervised by:
 * Date: 2014/07/24
 */

#include "riveting_controller/riveting_controller.h"
#include <boost/lexical_cast.hpp>

using namespace riveting_controller;

RivetingController::RivetingController(std::string group_name) :
    //listener_(tf_buffer_),
    group_name_(group_name), initialized_(false), home_frame_("rivet_home"), inverted_target_(false), /*robot_movement_client_(
     "/simulation_interface_ur5/cartesian_position", true),*/velocity_scale_(1.0), camera_frame_("stereo_camera_link"), rand_rotation_(
        1.0)
{

  srand(ros::Time::now().toNSec());

  listener_.reset(new tf::TransformListener(ros::Duration(5), true));

}

bool RivetingController::initialize()
{

  moveit::planning_interface::MoveGroup::Options ops(group_name_, "/simulation_ur5/robot_description_limited");

  ROS_INFO("creating move_group_interface");

  group_.reset(new move_group_interface::MoveGroup(ops));

  ROS_INFO("move_group_interface ready");

  planning_scene_monitor_.reset(
      new planning_scene_monitor::PlanningSceneMonitor("/simulation_ur5/robot_description_limited", listener_,
                                                       group_name_));

  ros::NodeHandle nh;
  planning_scene_pub_ = nh.advertise<moveit_msgs::PlanningScene>("/simulation_ur5/planning_scene", 1, false);

  goal_pub_ = nh.advertise<visualization_msgs::Marker>("goal_marker", 1, true);

  group_->startStateMonitor();

  ROS_INFO("Caching camera-end. ef. TF");

  if (listener_->waitForTransform(camera_frame_, group_->getEndEffectorLink(), ros::Time(0), ros::Duration(5.0)))
  {

    listener_->lookupTransform(camera_frame_, group_->getEndEffectorLink(), ros::Time(0), camera_to_endef_tf_);

    tf::StampedTransform tmp = camera_to_endef_tf_;

    camera_to_endef_tf_.setOrigin(tf::Vector3(tmp.getOrigin().getZ(), tmp.getOrigin().getY(), tmp.getOrigin().getX()));

  }
  else
  {

    camera_to_endef_tf_.setIdentity();
    ROS_ERROR("Error on getting transform to camera frame!!");

  }

  initialized_ = true;

  return true;

}

RivetingController::PlanningResult RivetingController::moveHome()
{

  PlanningResult res = moveToFrameOrigin(home_frame_);

  //rand_rotation_ = 1.0;
  if (res != PLANNED)
    return res;

  return executePlan();

}

bool RivetingController::frameToPose(std::string frame, geometry_msgs::PoseStamped& pose)
{

  pose.header.frame_id = frame;
  pose.header.stamp = ros::Time(0); // scene is static so this is legal

  pose.pose.position.x = 0.0;
  pose.pose.position.y = 0.0;
  pose.pose.position.z = 0.0;

  pose.pose.orientation.x = 0.0;
  pose.pose.orientation.y = 0.0;
  pose.pose.orientation.z = 0.0;
  pose.pose.orientation.w = 1.0;

  return true;

}

void RivetingController::setInvertedTarget(bool value)
{

  inverted_target_ = value;

}

RivetingController::PlanningResult RivetingController::approachToFrameOrigin(const geometry_msgs::PoseStamped& pose,
                                                                             const geometry_msgs::Vector3& offset,
                                                                             const bool& cam)
{

  if (!initialized_)
  {

    ROS_ERROR("Not initialized!");
    return OTHER_FAILURE;

  }

  ROS_INFO("Approaching to pose with frame_id: %s", pose.header.frame_id.c_str());

  //group_->setStartStateToCurrentState();

  geometry_msgs::PoseStamped ps = pose;

  ps.pose.position.x += offset.x;
  ps.pose.position.y += offset.y;
  ps.pose.position.z += offset.z;

  return moveToPoseStamped(ps, "RRTConnectkConfigDefault", cam);

}

RivetingController::PlanningResult RivetingController::approachToFrameOrigin(const std::string& frame,
                                                                             const geometry_msgs::Vector3& offset,
                                                                             const bool& cam)
{

  if (!initialized_)
  {

    ROS_ERROR("Not initialized!");
    return OTHER_FAILURE;

  }

  ROS_INFO("Approaching to pose: %s", frame.c_str());

  geometry_msgs::PoseStamped ps;

  if (!frameToPose(frame, ps))
    return OTHER_FAILURE;

  return approachToFrameOrigin(ps, offset, cam);

}

RivetingController::PlanningResult RivetingController::moveToFrameOrigin(const std::string& frame)
{

  if (!initialized_)
  {

    ROS_ERROR("Not initialized!");
    return OTHER_FAILURE;

  }

  geometry_msgs::PoseStamped ps;

  if (!frameToPose(frame, ps))
    return OTHER_FAILURE;

  ROS_INFO("Moving to: %s", frame.c_str());

  //group_->setStartStateToCurrentState();

  return moveToPoseStamped(ps, "RRTConnectkConfigDefault");

}

RivetingController::PlanningResult RivetingController::translateToFrameOrigin(const geometry_msgs::PoseStamped& pose,
                                                                              const geometry_msgs::Vector3& offset)
{

  if (!initialized_)
  {

    ROS_ERROR("Not initialized!");
    return OTHER_FAILURE;

  }

  ROS_INFO("Translating to pose with frame_id: %s", pose.header.frame_id.c_str());

  geometry_msgs::PoseStamped ps = pose;
  ps.header.stamp = ros::Time(0);

  // here we suppose that pose is in hole coordinate frame - maybe check it somehow?
  ps.pose.position.x += offset.x;
  ps.pose.position.y += offset.y;
  ps.pose.position.z += offset.z;

  if (!transformPose(group_->getPlanningFrame(), ps, ps))
    return OTHER_FAILURE;

  group_->setStartStateToCurrentState();
  ps.pose.orientation = group_->getCurrentPose().pose.orientation;

  // TODO (some) random rotation if planning inside hole fails??????????????????????????????

  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = group_->getEndEffectorLink();
  ocm.header.frame_id = ps.header.frame_id;
  ocm.orientation = ps.pose.orientation;
  ocm.absolute_x_axis_tolerance = 0.0001; // TODO experiment with these values
  ocm.absolute_y_axis_tolerance = 0.0001;
  ocm.absolute_z_axis_tolerance = M_PI;
  ocm.weight = 1.0;

  moveit_msgs::Constraints constraints;
  constraints.name = "tcp orientation";
  constraints.orientation_constraints.push_back(ocm);
  group_->setPathConstraints(constraints);

  velocity_scale_ = 0.1;

  // we are going to use alternative planner for translation as RRTConnect doesn't behave well (but otherwise is ok and fastest)
  PlanningResult res = moveToPoseStamped(ps, "PRMstarkConfigDefault" /*"RRTConnectkConfigDefault"*/, false, true, true,
                                         true);

  group_->clearPathConstraints();

  return res;

}

RivetingController::PlanningResult RivetingController::translateToFrameOrigin(const std::string& frame,
                                                                              const geometry_msgs::Vector3& offset)
{

  if (!initialized_)
  {

    ROS_ERROR("Not initialized!");
    return OTHER_FAILURE;

  }

  geometry_msgs::PoseStamped ps;

  if (!frameToPose(frame, ps)) // offset will be applied later
    return OTHER_FAILURE;

  return translateToFrameOrigin(ps, offset);

}

void RivetingController::clearObjects()
{

  ROS_INFO("Removing stored objects.");
  planning_scene_interface_.removeCollisionObjects(object_ids_);
  object_ids_.clear();

}

bool RivetingController::addCyllinder(const geometry_msgs::PoseStamped& pose, float height, float radius,
                                      bool allow_collisions)
{

  geometry_msgs::PoseStamped ps = pose;

  ps.header.stamp = ros::Time(0); // scene is static so this is legal

  if (!transformPose(group_->getPlanningFrame(), ps, ps))
    return false;

  moveit_msgs::CollisionObject obj;

  obj.id = "collision_object_" + boost::lexical_cast<std::string>(object_ids_.size());

  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.CYLINDER;
  primitive.dimensions.resize(2);
  primitive.dimensions[0] = height;
  primitive.dimensions[1] = radius;

  object_ids_.push_back(obj.id);

  /* A pose for the box (specified relative to frame_id) */

  obj.header.frame_id = ps.header.frame_id;
  obj.primitives.push_back(primitive);
  obj.primitive_poses.push_back(ps.pose);
  obj.operation = obj.ADD;

  std::vector<moveit_msgs::CollisionObject> collision_objects;
  collision_objects.push_back(obj);

  // Now, let's add the collision object into the world
  ROS_INFO("Add an object into the world");
  planning_scene_interface_.addCollisionObjects(collision_objects);

  if (allow_collisions)
  {

    if (!planning_scene_monitor_->requestPlanningSceneState("/simulation_ur5/get_planning_scene"))
      ROS_WARN("Error on getting current planning scene.");

    planning_scene_monitor::LockedPlanningSceneRW planning_scene = planning_scene_monitor::LockedPlanningSceneRW(
        planning_scene_monitor_);

    //planning_scene->getAllowedCollisionMatrixNonConst().setEntry(true); // TODO just test - disable collision checking completely

    planning_scene->getAllowedCollisionMatrixNonConst().setEntry(obj.id, true); // allow collisions with this object
    planning_scene->getAllowedCollisionMatrixNonConst().print(std::cout);

    moveit_msgs::PlanningScene ps;

    planning_scene->getPlanningSceneDiffMsg(ps);

    if (planning_scene_pub_.getNumSubscribers() == 0)
      ROS_ERROR("No one is listening to our planning scene! Double check topic name.");
    planning_scene_pub_.publish(ps);

  }

  return true;

}

bool RivetingController::addCyllinder(std::string frame, float height, float radius, bool allow_collisions)
{

  geometry_msgs::PoseStamped ps;

  if (!frameToPose(frame, ps))
    return false;

  return addCyllinder(ps, height, radius, allow_collisions);

}

void RivetingController::clearCollisionMap()
{

  if (!planning_scene_monitor_->requestPlanningSceneState("/simulation_ur5/get_planning_scene"))
  {

    ROS_WARN("Error on getting current planning scene.");
    return;

  }

  planning_scene_monitor::LockedPlanningSceneRW planning_scene = planning_scene_monitor::LockedPlanningSceneRW(
      planning_scene_monitor_);

  moveit_msgs::PlanningScene ps;

  planning_scene->getPlanningSceneMsg(ps);

  //ps.world.collision_objects.clear();
  ps.world.octomap.octomap.data.clear(); // TODO is this ok / sufficient?

  planning_scene_pub_.publish(ps);

}

bool RivetingController::transformPose(const std::string& target, const geometry_msgs::PoseStamped& in,
                                       geometry_msgs::PoseStamped& out)
{

  if (listener_->resolve(target) == listener_->resolve(in.header.frame_id))
  {

    out = in;
    return true;

  }

  if (!listener_->waitForTransform(target, in.header.frame_id, in.header.stamp, ros::Duration(3.0)))
  {

    ROS_ERROR("Can't transform %s frame to %s", in.header.frame_id.c_str(), target.c_str());
    return false;

  }

  try
  {

    listener_->transformPose(target, in, out);

  }
  catch (tf::TransformException& ex)
  {

    ROS_ERROR("TF exception: %s", ex.what());
    return false;

  }

  return true;

}

bool RivetingController::isStateValid(moveit::core::RobotState *robot_state,
                                      const moveit::core::JointModelGroup *joint_group,
                                      const double *joint_group_variable_values)
{

  planning_scene_monitor::LockedPlanningSceneRW planning_scene = planning_scene_monitor::LockedPlanningSceneRW(
      planning_scene_monitor_);

  robot_state->setJointGroupPositions(group_->getName(), joint_group_variable_values);

  collision_detection::CollisionRequest req;
  req.verbose = false;
  req.group_name = group_->getName();

  collision_detection::CollisionResult res;
  planning_scene->checkCollision(req, res, *robot_state); // planning_scene->isStateColliding ??
  if (res.collision)
    return false;

  return planning_scene->isStateFeasible(*robot_state);
}

RivetingController::PlanningResult RivetingController::moveToPoseStamped(const geometry_msgs::PoseStamped& pose,
                                                                         const std::string& planner, const bool& cam,
                                                                         const bool& no_transform,
                                                                         const bool& replanning,
                                                                         const bool& disable_pretest)
{

  ROS_ASSERT(pose.header.frame_id != "");

  if (!initialized_)
    return OTHER_FAILURE;

  geometry_msgs::PoseStamped ps = pose;

  if (no_transform == false)
  {

    tf::Pose tfpose;
    tf::poseMsgToTF(ps.pose, tfpose);
    if (pose.header.frame_id != home_frame_)
      tfpose.setRotation(tfpose.getRotation() * tf::Quaternion(tf::Vector3(0, 0, 1), rand_rotation_ * M_PI));
    tf::poseTFToMsg(tfpose, ps.pose);

  }

  if (!transformPose(group_->getPlanningFrame(), ps, ps))
    return OTHER_FAILURE;

  if (cam)
  {

    ROS_INFO("Use camera as end effector.");

    tf::Pose pstf;

    tf::poseMsgToTF(ps.pose, pstf);

    pstf = pstf * camera_to_endef_tf_;

    pstf.setRotation(pstf.getRotation() * tf::Quaternion(tf::Vector3(0, 1, 0), -M_PI / 2));
    pstf.setRotation(pstf.getRotation() * tf::Quaternion(tf::Vector3(1, 0, 0), M_PI));

    tf::poseTFToMsg(pstf, ps.pose);

  }

  if (inverted_target_ && (no_transform == false))
  {

    tf::Pose tfpose;

    tf::poseMsgToTF(ps.pose, tfpose);
    // TODO if planning fail - rotate it around x axis? add some rotation around z axis?
    tfpose.setRotation(tfpose.getRotation() * tf::Quaternion(tf::Vector3(0, 1, 0), M_PI));
    tfpose.setRotation(tfpose.getRotation() * tf::Quaternion(tf::Vector3(0, 0, 1), M_PI)); // TODO tmp workaround for camera-table collision
    tf::poseTFToMsg(tfpose, ps.pose);

  }

  visualization_msgs::Marker marker;

  marker.header = ps.header;
  marker.ns = "riveting_controller";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::ARROW;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose = ps.pose;
  marker.scale.x = 0.1;
  marker.scale.y = 0.02;
  marker.scale.z = 0.02;
  marker.color.r = 0.0f;
  marker.color.g = 1.0f;
  marker.color.b = 0.0f;
  marker.color.a = 1.0;
  marker.lifetime = ros::Duration();

  goal_pub_.publish(marker);

  if (!disable_pretest)
  {

    if (!planning_scene_monitor_->requestPlanningSceneState("/simulation_ur5/get_planning_scene"))
      ROS_WARN("Error on getting current planning scene.");

    robot_state::RobotStatePtr state = group_->getCurrentState();
    if (!state->setFromIK(state->getJointModelGroup(group_name_), ps.pose, group_->getEndEffectorLink(), 5, 0.0,
                          boost::bind(&RivetingController::isStateValid, this, _1, _2, _3)))
    {

      ROS_WARN("No valid IK for target pose.");
      return PLANNING_FAILED;

    }

  }

  group_->setPlannerId(planner);
  group_->setGoalPositionTolerance(0.00001);
  group_->setGoalOrientationTolerance(0.00001);
  group_->setPlanningTime(3.0);
  group_->allowReplanning(replanning);
  group_->setNumPlanningAttempts(6); // attempts are done in parallel (I hope)
  group_->setWorkspace(-1.0, -2.0, 0.5, 2.0, 2.0, 1.5);
  //group_->setEndEffectorLink("tcp");
  //group_->setEndEffector("end_eff");

  if (!group_->setPoseTarget(ps, "tcp"))
  {

    ROS_ERROR("Failed to set target pose.");
    return OTHER_FAILURE;

  }

  plan_.reset(new move_group_interface::MoveGroup::Plan);

  if (!group_->plan(*plan_))
  {

    plan_.reset();
    velocity_scale_ = 1.0;
    ROS_ERROR("Error on planning!");
    return PLANNING_FAILED; // TODO indicate this state to allow for instance slight change in pose and another try

  }

  scaleTrajectory(plan_, velocity_scale_);
  velocity_scale_ = 1.0;
  target_pose_ = ps;

  return PLANNED;

}

RivetingController::PlanningResult RivetingController::executePlan()
{

  if (!plan_)
    return OTHER_FAILURE;

  plan_.reset();

  ros::Time start(ros::Time::now());

  /*if (!group_->execute(*plan_))
  {

    plan_.reset();
    ROS_ERROR("Movement not finished (%f)!", (ros::Time::now()-start).toSec());
    return EXECUTION_FAILED;

  }*/

  if (!group_->move()) {

    ROS_ERROR("Movement not finished (%f)!", (ros::Time::now()-start).toSec());
    return EXECUTION_FAILED;

  }

  target_pose_.header.stamp = ros::Time(0);

  float err = poseFrameDist(target_pose_, "tcp");

  ROS_INFO("Movement finished (duration: %f, position error: %f)!", (ros::Time::now()-start).toSec(), err);

  //plan_.reset();
  return EXECUTED;

}

RivetingController::~RivetingController()
{

}

void RivetingController::scaleTrajectory(PlanPtr plan, float scale)
{

  if (scale == 1.0)
    return;

  if (!plan) {

    ROS_ERROR("Trajectory scaling: not a valid plan.");
    return;

  }

  ROS_INFO("Scaling trajectory (%f).",scale);

  for (unsigned int i = 0; i < plan->trajectory_.joint_trajectory.points.size(); i++)
  {

    plan->trajectory_.joint_trajectory.points[i].time_from_start = ros::Duration(
        plan->trajectory_.joint_trajectory.points[i].time_from_start.toSec() / scale);

    for (unsigned int j = 0; j < plan->trajectory_.joint_trajectory.joint_names.size(); j++)
    {

      plan->trajectory_.joint_trajectory.points[i].velocities[j] *= scale;
      plan->trajectory_.joint_trajectory.points[i].accelerations[j] *= (scale * scale);

    }

  }

}