interaction_handler.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012-2013, Willow Garage, Inc.
 *  Copyright (c) 2013, Ioan A. Sucan
 *  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 Willow Garage 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.
 *********************************************************************/

/* Author: Ioan Sucan, Adam Leeper */

#include <moveit/robot_interaction/interaction_handler.h>
#include <moveit/robot_interaction/robot_interaction.h>
#include <moveit/robot_interaction/interactive_marker_helpers.h>
#include <moveit/robot_interaction/kinematic_options_map.h>
#include <moveit/transforms/transforms.h>
#include <interactive_markers/interactive_marker_server.hpp>
#include <interactive_markers/menu_handler.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include <algorithm>

#include <Eigen/Core>
#include <Eigen/Geometry>

namespace robot_interaction
{
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_robot_interaction.interaction_handler");

InteractionHandler::InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
                                       const moveit::core::RobotState& initial_robot_state,
                                       const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
  : LockedRobotState(initial_robot_state)
  , name_(fixName(name))
  , planning_frame_(robot_interaction->getRobotModel()->getModelFrame())
  , tf_buffer_(tf_buffer)
  , kinematic_options_map_(robot_interaction->getKinematicOptionsMap())
  , display_meshes_(true)
  , display_controls_(true)
{
}

InteractionHandler::InteractionHandler(const RobotInteractionPtr& robot_interaction, const std::string& name,
                                       const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
  : LockedRobotState(robot_interaction->getRobotModel())
  , name_(fixName(name))
  , planning_frame_(robot_interaction->getRobotModel()->getModelFrame())
  , tf_buffer_(tf_buffer)
  , kinematic_options_map_(robot_interaction->getKinematicOptionsMap())
  , display_meshes_(true)
  , display_controls_(true)
{
}

std::string InteractionHandler::fixName(std::string name)
{
  std::replace(name.begin(), name.end(), '_', '-');  // we use _ as a special char in marker name
  return name;
}

void InteractionHandler::setPoseOffset(const EndEffectorInteraction& eef, const geometry_msgs::msg::Pose& m)
{
  boost::mutex::scoped_lock slock(offset_map_lock_);
  offset_map_[eef.eef_group] = m;
}

void InteractionHandler::setPoseOffset(const JointInteraction& vj, const geometry_msgs::msg::Pose& m)
{
  boost::mutex::scoped_lock slock(offset_map_lock_);
  offset_map_[vj.joint_name] = m;
}

void InteractionHandler::clearPoseOffset(const EndEffectorInteraction& eef)
{
  boost::mutex::scoped_lock slock(offset_map_lock_);
  offset_map_.erase(eef.eef_group);
}

void InteractionHandler::clearPoseOffset(const JointInteraction& vj)
{
  boost::mutex::scoped_lock slock(offset_map_lock_);
  offset_map_.erase(vj.joint_name);
}

void InteractionHandler::clearPoseOffsets()
{
  boost::mutex::scoped_lock slock(offset_map_lock_);
  offset_map_.clear();
}

bool InteractionHandler::getPoseOffset(const EndEffectorInteraction& eef, geometry_msgs::msg::Pose& m)
{
  boost::mutex::scoped_lock slock(offset_map_lock_);
  std::map<std::string, geometry_msgs::msg::Pose>::iterator it = offset_map_.find(eef.eef_group);
  if (it != offset_map_.end())
  {
    m = it->second;
    return true;
  }
  return false;
}

bool InteractionHandler::getPoseOffset(const JointInteraction& vj, geometry_msgs::msg::Pose& m)
{
  boost::mutex::scoped_lock slock(offset_map_lock_);
  std::map<std::string, geometry_msgs::msg::Pose>::iterator it = offset_map_.find(vj.joint_name);
  if (it != offset_map_.end())
  {
    m = it->second;
    return true;
  }
  return false;
}

bool InteractionHandler::getLastEndEffectorMarkerPose(const EndEffectorInteraction& eef,
                                                      geometry_msgs::msg::PoseStamped& ps)
{
  boost::mutex::scoped_lock slock(pose_map_lock_);
  std::map<std::string, geometry_msgs::msg::PoseStamped>::iterator it = pose_map_.find(eef.eef_group);
  if (it != pose_map_.end())
  {
    ps = it->second;
    return true;
  }
  return false;
}

bool InteractionHandler::getLastJointMarkerPose(const JointInteraction& vj, geometry_msgs::msg::PoseStamped& ps)
{
  boost::mutex::scoped_lock slock(pose_map_lock_);
  std::map<std::string, geometry_msgs::msg::PoseStamped>::iterator it = pose_map_.find(vj.joint_name);
  if (it != pose_map_.end())
  {
    ps = it->second;
    return true;
  }
  return false;
}

void InteractionHandler::clearLastEndEffectorMarkerPose(const EndEffectorInteraction& eef)
{
  boost::mutex::scoped_lock slock(pose_map_lock_);
  pose_map_.erase(eef.eef_group);
}

void InteractionHandler::clearLastJointMarkerPose(const JointInteraction& vj)
{
  boost::mutex::scoped_lock slock(pose_map_lock_);
  pose_map_.erase(vj.joint_name);
}

void InteractionHandler::clearLastMarkerPoses()
{
  boost::mutex::scoped_lock slock(pose_map_lock_);
  pose_map_.clear();
}

void InteractionHandler::setMenuHandler(const std::shared_ptr<interactive_markers::MenuHandler>& mh)
{
  boost::mutex::scoped_lock lock(state_lock_);
  menu_handler_ = mh;
}

const std::shared_ptr<interactive_markers::MenuHandler>& InteractionHandler::getMenuHandler()
{
  boost::mutex::scoped_lock lock(state_lock_);
  return menu_handler_;
}

void InteractionHandler::clearMenuHandler()
{
  boost::mutex::scoped_lock lock(state_lock_);
  menu_handler_.reset();
}

void InteractionHandler::handleGeneric(
    const GenericInteraction& g, const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback)
{
  if (g.process_feedback)
  {
    StateChangeCallbackFn callback;
    // modify the RobotState in-place with the state_lock_ held.
    LockedRobotState::modifyState(
        boost::bind(&InteractionHandler::updateStateGeneric, this, boost::placeholders::_1, &g, &feedback, &callback));

    // This calls update_callback_ to notify client that state changed.
    if (callback)
      callback(this);
  }
}

void InteractionHandler::handleEndEffector(
    const EndEffectorInteraction& eef,
    const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback)
{
  if (feedback->event_type != visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE)
    return;

  geometry_msgs::msg::PoseStamped tpose;
  geometry_msgs::msg::Pose offset;
  if (!getPoseOffset(eef, offset))
    offset.orientation.w = 1;
  if (transformFeedbackPose(feedback, offset, tpose))
  {
    pose_map_lock_.lock();
    pose_map_[eef.eef_group] = tpose;
    pose_map_lock_.unlock();
  }
  else
    return;

  StateChangeCallbackFn callback;

  // modify the RobotState in-place with state_lock_ held.
  // This locks state_lock_ before calling updateState()
  LockedRobotState::modifyState(boost::bind(&InteractionHandler::updateStateEndEffector, this, boost::placeholders::_1,
                                            &eef, &tpose.pose, &callback));

  // This calls update_callback_ to notify client that state changed.
  if (callback)
    callback(this);
}

void InteractionHandler::handleJoint(const JointInteraction& vj,
                                     const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback)
{
  if (feedback->event_type != visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE)
    return;

  geometry_msgs::msg::PoseStamped tpose;
  geometry_msgs::msg::Pose offset;
  if (!getPoseOffset(vj, offset))
    offset.orientation.w = 1;
  if (transformFeedbackPose(feedback, offset, tpose))
  {
    pose_map_lock_.lock();
    pose_map_[vj.joint_name] = tpose;
    pose_map_lock_.unlock();
  }
  else
    return;

  StateChangeCallbackFn callback;

  // modify the RobotState in-place with state_lock_ held.
  // This locks state_lock_ before calling updateState()
  LockedRobotState::modifyState(
      boost::bind(&InteractionHandler::updateStateJoint, this, boost::placeholders::_1, &vj, &tpose.pose, &callback));

  // This calls update_callback_ to notify client that state changed.
  if (callback)
    callback(this);
}

// MUST hold state_lock_ when calling this!
void InteractionHandler::updateStateGeneric(
    moveit::core::RobotState* state, const GenericInteraction* g,
    const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr* feedback, StateChangeCallbackFn* callback)
{
  bool ok = g->process_feedback(*state, *feedback);
  bool error_state_changed = setErrorState(g->marker_name_suffix, !ok);
  if (update_callback_)
    *callback = boost::bind(update_callback_, boost::placeholders::_1, error_state_changed);
}

// MUST hold state_lock_ when calling this!
void InteractionHandler::updateStateEndEffector(moveit::core::RobotState* state, const EndEffectorInteraction* eef,
                                                const geometry_msgs::msg::Pose* pose, StateChangeCallbackFn* callback)
{
  // This is called with state_lock_ held, so no additional locking needed to
  // access kinematic_options_map_.
  KinematicOptions kinematic_options = kinematic_options_map_->getOptions(eef->parent_group);

  bool ok = kinematic_options.setStateFromIK(*state, eef->parent_group, eef->parent_link, *pose);
  bool error_state_changed = setErrorState(eef->parent_group, !ok);
  if (update_callback_)
    *callback = boost::bind(update_callback_, boost::placeholders::_1, error_state_changed);
}

// MUST hold state_lock_ when calling this!
void InteractionHandler::updateStateJoint(moveit::core::RobotState* state, const JointInteraction* vj,
                                          const geometry_msgs::msg::Pose* feedback_pose,
                                          StateChangeCallbackFn* callback)
{
  Eigen::Isometry3d pose;
  tf2::fromMsg(*feedback_pose, pose);

  if (!vj->parent_frame.empty() && !moveit::core::Transforms::sameFrame(vj->parent_frame, planning_frame_))
    pose = state->getGlobalLinkTransform(vj->parent_frame).inverse() * pose;

  state->setJointPositions(vj->joint_name, pose);
  state->update();

  if (update_callback_)
    *callback = boost::bind(update_callback_, boost::placeholders::_1, false);
}

bool InteractionHandler::inError(const EndEffectorInteraction& eef) const
{
  return getErrorState(eef.parent_group);
}

bool InteractionHandler::inError(const GenericInteraction& g) const
{
  return getErrorState(g.marker_name_suffix);
}

bool InteractionHandler::inError(const JointInteraction& /*unused*/) const
{
  return false;
}

void InteractionHandler::clearError()
{
  boost::mutex::scoped_lock lock(state_lock_);
  error_state_.clear();
}

// return true if the state changed.
// MUST hold state_lock_ when calling this!
bool InteractionHandler::setErrorState(const std::string& name, bool new_error_state)
{
  bool old_error_state = error_state_.find(name) != error_state_.end();

  if (new_error_state == old_error_state)
    return false;

  if (new_error_state)
    error_state_.insert(name);
  else
    error_state_.erase(name);

  return true;
}

bool InteractionHandler::getErrorState(const std::string& name) const
{
  boost::mutex::scoped_lock lock(state_lock_);
  return error_state_.find(name) != error_state_.end();
}

bool InteractionHandler::transformFeedbackPose(
    const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback,
    const geometry_msgs::msg::Pose& offset, geometry_msgs::msg::PoseStamped& tpose)
{
  tpose.header = feedback->header;
  tpose.pose = feedback->pose;
  if (feedback->header.frame_id != planning_frame_)
  {
    if (tf_buffer_)
      try
      {
        geometry_msgs::msg::PoseStamped spose(tpose);
        // Express feedback (marker) pose in planning frame
        tf_buffer_->transform(tpose, spose, planning_frame_);
        // Apply inverse of offset to bring feedback pose back into the end-effector support link frame
        tf2::Transform tf_offset, tf_tpose;
        tf2::fromMsg(offset, tf_offset);
        tf2::fromMsg(spose.pose, tf_tpose);
        tf2::toMsg(tf_tpose * tf_offset.inverse(), tpose.pose);
      }
      catch (tf2::TransformException& e)
      {
        RCLCPP_ERROR(LOGGER, "Error transforming from frame '%s' to frame '%s'", tpose.header.frame_id.c_str(),
                     planning_frame_.c_str());
        return false;
      }
    else
    {
      RCLCPP_ERROR(LOGGER, "Cannot transform from frame '%s' to frame '%s' (no TF instance provided)",
                   tpose.header.frame_id.c_str(), planning_frame_.c_str());
      return false;
    }
  }
  return true;
}

void InteractionHandler::setUpdateCallback(const InteractionHandlerCallbackFn& callback)
{
  boost::mutex::scoped_lock lock(state_lock_);
  update_callback_ = callback;
}

const InteractionHandlerCallbackFn& InteractionHandler::getUpdateCallback() const
{
  boost::mutex::scoped_lock lock(state_lock_);
  return update_callback_;
}

void InteractionHandler::setMeshesVisible(bool visible)
{
  boost::mutex::scoped_lock lock(state_lock_);
  display_meshes_ = visible;
}

bool InteractionHandler::getMeshesVisible() const
{
  boost::mutex::scoped_lock lock(state_lock_);
  return display_meshes_;
}

void InteractionHandler::setControlsVisible(bool visible)
{
  boost::mutex::scoped_lock lock(state_lock_);
  display_controls_ = visible;
}

bool InteractionHandler::getControlsVisible() const
{
  boost::mutex::scoped_lock lock(state_lock_);
  return display_controls_;
}
}  // namespace robot_interaction