imarker_end_effector.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016, University of Colorado, Boulder
 *  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 PickNik LLC 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: Dave Coleman
   Desc:   Class to encapsule a visualized robot state that can be controlled using an interactive marker
*/

// Boost
#include <boost/filesystem.hpp>

// C++
#include <string>

// Conversions
#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>

// this package
#include <moveit_visual_tools/imarker_robot_state.h>
#include <moveit_visual_tools/imarker_end_effector.h>

namespace moveit_visual_tools
{
IMarkerEndEffector::IMarkerEndEffector(IMarkerRobotState* imarker_parent, const std::string& imarker_name,
                                       ArmData arm_data, rviz_visual_tools::colors color)
  : name_(imarker_name)
  , imarker_parent_(imarker_parent)
  , psm_(imarker_parent_->psm_)
  , arm_data_(arm_data)
  , color_(color)
  , imarker_server_(imarker_parent_->imarker_server_)
  , imarker_state_(imarker_parent_->imarker_state_)
  , visual_tools_(imarker_parent_->visual_tools_)
{
  // Get pose from robot state
  imarker_pose_ = imarker_state_->getGlobalLinkTransform(arm_data_.ee_link_);

  // Create imarker
  initializeInteractiveMarkers();

  ROS_INFO_STREAM_NAMED(name_, "IMarkerEndEffector '" << name_ << "' tracking ee link '"
                                                      << arm_data_.ee_link_->getName() << "' ready.");
}

void IMarkerEndEffector::getPose(Eigen::Affine3d& pose)
{
  pose = imarker_pose_;
}

bool IMarkerEndEffector::setPoseFromRobotState()
{
  imarker_pose_ = imarker_state_->getGlobalLinkTransform(arm_data_.ee_link_);

  sendUpdatedIMarkerPose();

  return true;
}

void IMarkerEndEffector::iMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
{
  if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP)
  {
    // Save pose to file if its been long enough
    double save_every_sec = 0.1;
    if (time_since_last_save_ < ros::Time::now() - ros::Duration(save_every_sec))
    {
      imarker_parent_->saveToFile();
      time_since_last_save_ = ros::Time::now();
    }
    return;
  }

  // Ignore if not pose update
  if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
    return;

  // Only allow one feedback to be processed at a time
  {
    // boost::unique_lock<boost::mutex> scoped_lock(imarker_mutex_);
    if (imarker_ready_to_process_ == false)
    {
      return;
    }
    imarker_ready_to_process_ = false;
  }

  // Convert
  Eigen::Affine3d robot_ee_pose;
  tf::poseMsgToEigen(feedback->pose, robot_ee_pose);

  // Update robot
  solveIK(robot_ee_pose);

  // Redirect to base class
  if (imarker_callback_)
    imarker_callback_(feedback, robot_ee_pose);

  // Allow next feedback to be processed
  {
    // boost::unique_lock<boost::mutex> scoped_lock(imarker_mutex_);
    imarker_ready_to_process_ = true;
  }
}

void IMarkerEndEffector::solveIK(Eigen::Affine3d& pose)
{
  // Cartesian settings
  const std::size_t attempts = 2;
  const double timeout = 1.0 / 30.0;  // 30 fps

  // Optionally collision check
  moveit::core::GroupStateValidityCallbackFn constraint_fn;
  if (use_collision_checking_)
  {
    // TODO(davetcoleman): this is currently not working, the locking seems to cause segfaults
    boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
    ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(psm_));
    constraint_fn = boost::bind(&isStateValid, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(),
                                collision_checking_verbose_, only_check_self_collision_, visual_tools_, _1, _2, _3);
  }

  // Attempt to set robot to new pose
  if (imarker_state_->setFromIK(arm_data_.jmg_, pose, arm_data_.ee_link_->getName(), attempts, timeout, constraint_fn))
  {
    imarker_state_->update();
    // if (psm_->getPlanningScene()->isStateValid(*imarker_state_))
    //{
    // ROS_INFO_STREAM_NAMED(name_, "Solved IK");
    imarker_parent_->publishRobotState();
    //}
    // else
    // {
    //   visual_tools_->publishRobotState(imarker_state_, rviz_visual_tools::RED);
    //   exit(0);
    // }
  }
}

void IMarkerEndEffector::initializeInteractiveMarkers()
{
  // Convert
  geometry_msgs::Pose pose_msg;
  tf::poseEigenToMsg(imarker_pose_, pose_msg);

  // marker
  make6DofMarker(pose_msg);
}

void IMarkerEndEffector::updateIMarkerPose(const Eigen::Affine3d& pose)
{
  // Move marker to tip of fingers
  // imarker_pose_ = pose * imarker_offset_.inverse();
  sendUpdatedIMarkerPose();
}

void IMarkerEndEffector::sendUpdatedIMarkerPose()
{
  // Convert
  geometry_msgs::Pose pose_msg;
  tf::poseEigenToMsg(imarker_pose_, pose_msg);

  imarker_server_->setPose(int_marker_.name, pose_msg);
  imarker_server_->applyChanges();
}

void IMarkerEndEffector::make6DofMarker(const geometry_msgs::Pose& pose)
{
  ROS_DEBUG_STREAM_NAMED(name_, "Making 6dof interactive marker named " << name_);

  int_marker_.header.frame_id = "world";
  int_marker_.pose = pose;
  int_marker_.scale = 0.2;

  int_marker_.name = name_;
  // int_marker_.description = "imarker_" + name_; // TODO: unsure, but I think this causes a caption in Rviz that I
  // don't want

  // insert a box
  // makeBoxControl(int_marker_);

  // int_marker_.controls[0].interaction_mode = InteractiveMarkerControl::MENU;

  InteractiveMarkerControl control;
  control.orientation.w = 1;
  control.orientation.x = 1;
  control.orientation.y = 0;
  control.orientation.z = 0;
  control.name = "rotate_x";
  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
  int_marker_.controls.push_back(control);
  control.name = "move_x";
  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
  int_marker_.controls.push_back(control);

  control.orientation.w = 1;
  control.orientation.x = 0;
  control.orientation.y = 1;
  control.orientation.z = 0;
  control.name = "rotate_z";
  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
  int_marker_.controls.push_back(control);
  control.name = "move_z";
  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
  int_marker_.controls.push_back(control);

  control.orientation.w = 1;
  control.orientation.x = 0;
  control.orientation.y = 0;
  control.orientation.z = 1;
  control.name = "rotate_y";
  control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
  int_marker_.controls.push_back(control);
  control.name = "move_y";
  control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
  int_marker_.controls.push_back(control);

  imarker_server_->insert(int_marker_);
  imarker_server_->setCallback(int_marker_.name, boost::bind(&IMarkerEndEffector::iMarkerCallback, this, _1));
  // menu_handler_.apply(*imarker_server_, int_marker_.name);
}

visualization_msgs::InteractiveMarkerControl&
IMarkerEndEffector::makeBoxControl(visualization_msgs::InteractiveMarker& msg)
{
  visualization_msgs::InteractiveMarkerControl control;
  control.always_visible = true;

  visualization_msgs::Marker marker;
  marker.type = visualization_msgs::Marker::CUBE;
  marker.scale.x = msg.scale * 0.3;   // x direction
  marker.scale.y = msg.scale * 0.10;  // y direction
  marker.scale.z = msg.scale * 0.10;  // height
  marker.color.r = 0.5;
  marker.color.g = 0.5;
  marker.color.b = 0.5;
  marker.color.a = 1.0;

  control.markers.push_back(marker);
  msg.controls.push_back(control);

  return msg.controls.back();
}

}  // namespace moveit_visual_tools

namespace
{
bool isStateValid(const planning_scene::PlanningScene* planning_scene, bool verbose, bool only_check_self_collision,
                  moveit_visual_tools::MoveItVisualToolsPtr visual_tools, moveit::core::RobotState* robot_state,
                  const moveit::core::JointModelGroup* group, const double* ik_solution)
{
  // Apply IK solution to robot state
  robot_state->setJointGroupPositions(group, ik_solution);
  robot_state->update();

  // Ensure there are objects in the planning scene
  if (false)
  {
    const std::size_t num_collision_objects = planning_scene->getCollisionWorld()->getWorld()->size();
    if (num_collision_objects == 0)
    {
      ROS_ERROR_STREAM_NAMED("cart_path_planner", "No collision objects exist in world, you need at least a table "
                                                  "modeled for the controller to work");
      ROS_ERROR_STREAM_NAMED("cart_path_planner", "To fix this, relaunch the teleop/head tracking/whatever MoveIt! "
                                                  "node to publish the collision objects");
      return false;
    }
  }

  if (!planning_scene)
  {
    ROS_ERROR_STREAM_NAMED("cart_path_planner", "No planning scene provided");
    return false;
  }
  if (only_check_self_collision)
  {
    // No easy API exists for only checking self-collision, so we do it here.
    // TODO(davetcoleman): move this into planning_scene.cpp
    collision_detection::CollisionRequest req;
    req.verbose = verbose;
    req.group_name = group->getName();
    collision_detection::CollisionResult res;
    planning_scene->checkSelfCollision(req, res, *robot_state);
    if (!res.collision)
      return true;  // not in collision
  }
  else if (!planning_scene->isStateColliding(*robot_state, group->getName()))
    return true;  // not in collision

  // Display more info about the collision
  if (verbose)
  {
    visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED);
    planning_scene->isStateColliding(*robot_state, group->getName(), true);
    visual_tools->publishContactPoints(*robot_state, planning_scene);
  }
  ROS_WARN_STREAM_THROTTLE_NAMED(2.0, "cart_path_planner", "Collision");
  return false;
}

}  // end annonymous namespace