move_arm_utils.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, 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 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: Matthew Klingensmith */
#ifndef MOVE_ARM_UTILS_H
#define MOVE_ARM_UTILS_H
#include <planning_environment/models/collision_models.h>
#include <arm_navigation_msgs/PlanningScene.h>
#include <arm_navigation_msgs/SetPlanningSceneDiff.h>
#include <planning_environment/models/model_utils.h>
#include <rosgraph_msgs/Clock.h>
#include <std_msgs/ColorRGBA.h>
#include <tf/transform_broadcaster.h>
#include <kinematics_msgs/GetConstraintAwarePositionIK.h>
#include <kinematics_msgs/GetPositionIK.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/simple_client_goal_state.h>
#include <arm_navigation_msgs/GetMotionPlan.h>
#include <arm_navigation_msgs/GetStateValidity.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
#include <arm_navigation_msgs/convert_messages.h>
//#include <move_arm_warehouse/move_arm_warehouse_logger_reader.h>
#include <srs_assisted_arm_navigation/move_arm_utils/move_arm_warehouse_logger_reader.h>
#include <arm_navigation_msgs/convert_messages.h>
#include <visualization_msgs/InteractiveMarker.h>
#include <interactive_markers/interactive_marker_server.h>
#include <interactive_markers/menu_handler.h>
#include <interactive_markers/tools.h>
#include <arm_navigation_msgs/CollisionObject.h>
#include <planning_environment/monitors/kinematic_model_state_monitor.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
//#include <gazebo_msgs/SetModelConfiguration.h>
#include <std_srvs/Empty.h>
#include <pr2_mechanism_msgs/ListControllers.h>
#include <pr2_mechanism_msgs/LoadController.h>
#include <pr2_mechanism_msgs/UnloadController.h>
#include <pr2_mechanism_msgs/SwitchController.h>
#include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
// #include <gazebo_msgs/SetLinkProperties.h>
// #include <gazebo_msgs/GetLinkProperties.h>

typedef map<std::string, interactive_markers::MenuHandler::EntryHandle> MenuEntryMap;
typedef map<std::string, MenuEntryMap> MenuMap;
typedef map<std::string, interactive_markers::MenuHandler> MenuHandlerMap;

namespace planning_scene_utils
{

inline static geometry_msgs::Pose toGeometryPose(tf::Transform transform)
{
  geometry_msgs::Pose toReturn;
  toReturn.position.x = transform.getOrigin().x();
  toReturn.position.y = transform.getOrigin().y();
  toReturn.position.z = transform.getOrigin().z();
  toReturn.orientation.x = transform.getRotation().x();
  toReturn.orientation.y = transform.getRotation().y();
  toReturn.orientation.z = transform.getRotation().z();
  toReturn.orientation.w = transform.getRotation().w();
  return toReturn;
}

inline static tf::Transform toBulletTransform(geometry_msgs::Pose pose)
{
  tf::Quaternion quat =
    tf::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
  tf::Vector3 vec = tf::Vector3(pose.position.x, pose.position.y, pose.position.z);
  return tf::Transform(quat, vec);
}

inline static std::string getPlanningSceneNameFromId(const unsigned int id) {
  std::stringstream ss;
  ss << "Planning Scene " << id;
  return ss.str();
}

inline static unsigned int getPlanningSceneIdFromName(const std::string& name) {
  std::stringstream ss(name);
  std::string temp;
  ss >> temp;
  ss >> temp;
  unsigned int ret;
  ss >> ret;
  return ret;
}

inline static std::string getMotionPlanRequestNameFromId(const unsigned int id) {
  std::stringstream ss;
  ss << "MPR " << id;
  return ss.str();
}

inline static std::string getTrajectoryNameFromId(const unsigned int id) {
  std::stringstream ss;
  ss << "Trajectory " << id;
  return ss.str();
}

inline static std::string getResultErrorFromCode(int error_code)
{
  std::string result;
  if(error_code == control_msgs::FollowJointTrajectoryResult::SUCCESSFUL)
     result = "Success";
  else if(error_code == control_msgs::FollowJointTrajectoryResult::INVALID_GOAL)
     result = "Invalid Goal";
  else if(error_code == control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS)
     result = "Invalid Joints";
  else if(error_code == control_msgs::FollowJointTrajectoryResult::OLD_HEADER_TIMESTAMP)
     result = "Old header timestamp";
  else if(error_code == control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED)
     result = "Path tolerance violated";
  else if(error_code == control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED)
     result = "Goal tolerance violated";
  return result;
}

enum PositionType
  {
    StartPosition, GoalPosition
  };

enum RenderType
  {
    CollisionMesh, VisualMesh, PaddingMesh
  };

enum TrajectoryRenderType
{
  Kinematic,
  Temporal,
};


// Must be defined so that subsequent classes can reference.
class PlanningSceneEditor;

class PlanningSceneData
{
protected:
  std::string name_;
  unsigned int id_;
  std::string host_;
  ros::Time timestamp_;
  arm_navigation_msgs::PlanningScene planning_scene_;
  std::vector<std::string> pipeline_stages_;
  std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> error_codes_;
  std::set<unsigned int> motion_plan_requests_;

public:
  PlanningSceneData();
  PlanningSceneData(unsigned int id, const ros::Time& timestamp, const arm_navigation_msgs::PlanningScene& scene);

  inline std::string getHostName() const
  {
    return host_;
  }

  inline void setHostName(std::string host)
  {
    host_ = host;
  }

  inline std::string getName() const
  {
    return name_;
  }

  inline unsigned int getId() const 
  {
    return id_;
  }

  inline const ros::Time& getTimeStamp() const
  {
    return timestamp_;
  }

  inline arm_navigation_msgs::PlanningScene& getPlanningScene()
  {
    return planning_scene_;
  }

  inline void setTimeStamp(const ros::Time& time)
  {
    timestamp_ = time;
    planning_scene_.robot_state.joint_state.header.stamp = time;
  }

  inline void setId(unsigned int id) {
    id_ = id;
    name_ = getPlanningSceneNameFromId(id);
  }

  inline void setPlanningScene(const arm_navigation_msgs::PlanningScene& scene)
  {
    planning_scene_ = scene;
    timestamp_ = scene.robot_state.joint_state.header.stamp;
  }

  inline std::vector<std::string>& getPipelineStages()
  {
    return pipeline_stages_;
  }

  inline void setPipelineStages(std::vector<std::string>& stages)
  {
    pipeline_stages_ = stages;
  }

  inline std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& getErrorCodes()
  {
    return error_codes_;
  }

  inline void setErrorCodes(std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes)
  {
    error_codes_ = error_codes;
  }

  inline std::set<unsigned int>& getRequests()
  {
    return motion_plan_requests_;
  }

  void getRobotState(planning_models::KinematicState* state);

  bool hasMotionPlanRequestId(unsigned int id) const {
    return(motion_plan_requests_.find(id) != motion_plan_requests_.end());
  }
  
  void addMotionPlanRequestId(unsigned int id) {
    motion_plan_requests_.insert(id);
  }

  void removeMotionPlanRequestId(unsigned int id) {
    motion_plan_requests_.erase(id);
  }

  unsigned int getNextMotionPlanRequestId() const {
    if(motion_plan_requests_.empty()) {
      return 0;
    }
    return (*motion_plan_requests_.rbegin())+1;
  }
};

class MotionPlanRequestData
{
protected:
  std::string name_;
  unsigned int id_;
  std::string source_;
  unsigned int planning_scene_id_;
  std::string end_effector_link_;
  std::string group_name_;
  arm_navigation_msgs::MotionPlanRequest motion_plan_request_;
  bool is_start_editable_;
  bool is_goal_editable_;
  bool is_start_visible_;
  bool is_goal_visible_;
  bool should_refresh_colors_;
  bool has_refreshed_colors_;
  bool has_path_constraints_;
  bool has_good_goal_ik_solution_;
  bool has_good_start_ik_solution_;
  bool are_collisions_visible_;
  bool has_state_changed_;
  bool are_joint_controls_visible_;

  double roll_tolerance_;
  double pitch_tolerance_;
  double yaw_tolerance_;

  bool constrain_roll_;
  bool constrain_pitch_;
  bool constrain_yaw_;

  std_msgs::ColorRGBA start_color_;
  std_msgs::ColorRGBA goal_color_;
  std::set<unsigned int> trajectories_;
  planning_models::KinematicState* start_state_;
  planning_models::KinematicState* goal_state_;
  tf::Transform last_good_start_pose_;
  tf::Transform last_good_goal_pose_;
  visualization_msgs::MarkerArray collision_markers_;
  RenderType render_type_;
  unsigned int next_trajectory_id_;

public:
  MotionPlanRequestData()
  {
    start_state_ = NULL;
    goal_state_ = NULL;
  }

  MotionPlanRequestData(const planning_models::KinematicState* robot_state);
  MotionPlanRequestData(const unsigned int& id, 
                        const std::string& source, 
                        const arm_navigation_msgs::MotionPlanRequest& request,
                        const planning_models::KinematicState* robot_state, 
                        const std::string& end_effector_link);

  ros::Duration refresh_timer_;


  void setStartStateValues(std::map<std::string, double>& joint_values);

  void setGoalStateValues(std::map<std::string, double>& joint_values);

  void updateStartState();

  void updateGoalState();

  std::vector<std::string> getJointNamesInGoal();

  bool isJointNameInGoal(std::string joint);

  inline RenderType getRenderType() const
  {
    return render_type_;
  }

  inline void setRenderType(RenderType renderType)
  {
    render_type_ = renderType;
  }

  inline bool areJointControlsVisible() const
  {
    return are_joint_controls_visible_;
  }

  void setJointControlsVisible(bool visible, PlanningSceneEditor* editor);

  inline bool hasStateChanged() const
  {
    return has_state_changed_;
  }

  inline void setStateChanged(bool changed)
  {
    has_state_changed_ = changed;
  }

  inline visualization_msgs::MarkerArray& getCollisionMarkers()
  {
    return collision_markers_;
  }

  inline bool areCollisionsVisible() const
  {
    return are_collisions_visible_;
  }

  inline void setCollisionsVisible(bool visible)
  {
    are_collisions_visible_ = visible;
  }

  inline void showCollisions() 
  {
    setCollisionsVisible(true);
  }

  inline void hideCollisions()
  {
    setCollisionsVisible(false);
  }

  inline tf::Transform getLastGoodStartPose() const
  {
    return last_good_start_pose_;
  }

  inline tf::Transform getLastGoodGoalPose() const
  {
    return last_good_goal_pose_;
  }

  inline void setLastGoodStartPose(tf::Transform pose)
  {
    last_good_start_pose_ = pose;
  }

  inline void setLastGoodGoalPose(tf::Transform pose)
  {
    last_good_goal_pose_ = pose;
  }

  inline void setStartState(planning_models::KinematicState* state)
  {
    start_state_ = state;
    setStateChanged(true);
  }

  inline void setGoalState(planning_models::KinematicState* state)
  {
    goal_state_ = state;
    setStateChanged(true);
  }

  inline void reset()
  {
    if(start_state_ != NULL)
    {
      delete start_state_;
      start_state_ = NULL;
    }

    if(goal_state_ != NULL)
    {
      delete goal_state_;
      goal_state_ = NULL;
    }
  }

  inline bool hasGoodIKSolution(const PositionType& type) const
  {
    if(type == StartPosition) {
      return has_good_start_ik_solution_;
    } else {
      return has_good_goal_ik_solution_;
    }
  }

  inline void setHasGoodIKSolution(const bool& solution, const PositionType& type)
  {
    if(type == StartPosition) {
      has_good_start_ik_solution_ = solution;
    } else {
      has_good_goal_ik_solution_ = solution;
    }
  }

  inline planning_models::KinematicState* getStartState()
  {
    return start_state_;
  }

  inline planning_models::KinematicState* getGoalState()
  {
    return goal_state_;
  }

  inline std::string getGroupName() const
  { 
    return group_name_;
  }

  inline std::string getEndEffectorLink() const
  {
    return end_effector_link_;
  }

  inline void setGroupName(const std::string& name)
  {
    group_name_ = name;
  }

  inline void setEndEffectorLink(const std::string& name)
  {
    end_effector_link_ = name;
  }

  inline const std::string& getName() const
  {
    return name_;
  }
  
  unsigned int getId() const {
    return id_;
  }

  inline void setId(const unsigned int id)
  {
    id_ = id;
    name_ = getMotionPlanRequestNameFromId(id_);
  }

  inline bool shouldRefreshColors() const 
  {
    return should_refresh_colors_;
  }

  inline bool hasRefreshedColors() const
  {
    return has_refreshed_colors_;
  }

  inline void setHasRefreshedColors(bool refresh)
  {
    has_refreshed_colors_ = refresh;

    if(refresh)
    {
      should_refresh_colors_ = false;
    }
  }

  inline void refreshColors()
  {
    should_refresh_colors_ = true;
    has_refreshed_colors_ = false;
    refresh_timer_ = ros::Duration(0.0);
  }

  inline bool isStartVisible() const
  {
    return is_start_visible_;
  }

  inline bool isEndVisible() const
  {
    return is_goal_visible_;
  }

  inline void setStartVisible(bool visible)
  {
    is_start_visible_ = visible;
  }

  inline void setEndVisible(bool visible)
  {
    is_goal_visible_ = visible;
  }

  inline void show()
  {
    setStartVisible(true);
    setEndVisible(true);
  }

  inline void hide()
  {
    setStartVisible(false);
    setEndVisible(false);
  }

  inline void showStart()
  {
    setStartVisible(true);
  }

  inline void showGoal()
  {
    setEndVisible(true);
  }

  inline void hideStart()
  {
    setStartVisible(false);
  }

  inline void hideGoal()
  {
    setEndVisible(false);
  }

  inline const std_msgs::ColorRGBA& getStartColor() const
  {
    return start_color_;
  }

  inline const std_msgs::ColorRGBA& getGoalColor() const
  {
    return goal_color_;
  }

  inline void setStartColor(std_msgs::ColorRGBA color)
  {
    start_color_ = color;
  }

  inline void setGoalColor(std_msgs::ColorRGBA color)
  {
    goal_color_ = color;
  }

  inline void setStartEditable(bool editable)
  {
    is_start_editable_ = editable;
  }

  inline void setGoalEditable(bool editable)
  {
    is_goal_editable_ = editable;
  }

  inline bool isStartEditable() const
  {
    return is_start_editable_;
  }

  inline bool isGoalEditable() const
  {
    return is_goal_editable_;
  }

  inline void setSource(const std::string& source)
  {
    source_ = source;
  }

  inline const std::string& getSource() const
  {
    return source_;
  }

  inline arm_navigation_msgs::MotionPlanRequest& getMotionPlanRequest()
  {
    return motion_plan_request_;
  }

  inline void setMotionPlanRequest(const arm_navigation_msgs::MotionPlanRequest& request)
  {
    motion_plan_request_ = request;
    setGroupName(request.group_name);
    updateStartState();
    updateGoalState();
  }

  inline void setPlanningSceneId(const unsigned int id)
  {
    planning_scene_id_ = id;
  }
  inline unsigned int getPlanningSceneId() const
  {
    return planning_scene_id_;
  }

  inline std::string getPlanningSceneName() const 
  {
    return getPlanningSceneNameFromId(planning_scene_id_);
  }

  inline std::set<unsigned int>& getTrajectories()
  {
    return trajectories_;
  }

  unsigned int getNextTrajectoryId() const {
    if(trajectories_.empty()) return 0;
    return (*trajectories_.rbegin())+1;
  }

  void addTrajectoryId(unsigned int id) {
    trajectories_.insert(id);
  }
  
  void removeTrajectoryId(unsigned int id) {
    trajectories_.erase(id);
  }

  bool hasTrajectoryId(unsigned int id) const {
    return (trajectories_.find(id) != trajectories_.end());
  }

  bool hasPathConstraints() const {
    return has_path_constraints_;
  }
  
  void setPathConstraints(bool has) {
    has_path_constraints_ = has;
  }

  bool getConstrainRoll() const {
    return constrain_roll_;
  }

  void setConstrainRoll(bool s) {
    constrain_roll_ = s;
  }

  bool getConstrainPitch() const {
    return constrain_pitch_;
  }

  void setConstrainPitch(bool s) {
    constrain_pitch_ = s;
  }

  bool getConstrainYaw() const {
    return constrain_yaw_;
  }

  void setConstrainYaw(bool s) {
    constrain_yaw_ = s;
  }

  double getRollTolerance() const {
    return roll_tolerance_;
  }

  void setRollTolerance(double s) {
    roll_tolerance_ = s;
  }

  double getPitchTolerance() const {
    return pitch_tolerance_;
  }

  void setPitchTolerance(double s) {
    pitch_tolerance_ = s;
  }

  double getYawTolerance() const {
    return yaw_tolerance_;
  }

  void setYawTolerance(double s) {
    yaw_tolerance_ = s;
  }

  void setGoalAndPathPositionOrientationConstraints(arm_navigation_msgs::MotionPlanRequest& mpr,
                                                    planning_scene_utils::PositionType type) const;

  void updateCollisionMarkers(planning_environment::CollisionModels* cm_,
                              ros::ServiceClient* distance_state_validity_service_client_);
};


class TrajectoryData
{
public:

  enum MarkerType {
    VISUAL,
    COLLISION,
    PADDED
  };

protected:
  std::string name_;
  unsigned int id_;
  std::string source_;
  std::string group_name_;
  unsigned int planning_scene_id_;
  unsigned int motion_plan_request_Id_;
  trajectory_msgs::JointTrajectory trajectory_;
  trajectory_msgs::JointTrajectory trajectory_error_;
  bool is_visible_;
  MarkerType marker_type_;
  bool is_playing_;
  ros::Time playback_start_time_;
  bool collisions_visible_;
  bool state_changed_;
  std_msgs::ColorRGBA color_;
  unsigned int current_trajectory_point_;
  unsigned int trajectory_bad_point_;
  planning_models::KinematicState* current_state_;
  ros::Duration duration_;
  bool should_refresh_colors_;
  bool has_refreshed_colors_;
  visualization_msgs::MarkerArray collision_markers_;
  RenderType render_type_;
  TrajectoryRenderType trajectory_render_type_;
  ros::Duration time_to_stop_;
public:

  ros::Duration refresh_timer_;

  arm_navigation_msgs::ArmNavigationErrorCodes trajectory_error_code_;

  TrajectoryData();
  TrajectoryData(const unsigned int& id, const std::string& source, const std::string& group_name,
                 const trajectory_msgs::JointTrajectory& trajectory);
  TrajectoryData(const unsigned int& id, const std::string& source, const std::string& group_name,
                 const trajectory_msgs::JointTrajectory& trajectory, const trajectory_msgs::JointTrajectory& trajectory_error);

  void advanceThroughTrajectory(int amount);

  void advanceToNextClosestPoint(ros::Time time);

  void updateCurrentState();

  inline bool hasStateChanged() const
  { 
    return state_changed_;
  }

  inline void setStateChanged(bool changed)
  {
    state_changed_ = changed;
  }

  inline visualization_msgs::MarkerArray& getCollisionMarkers()
  {
    return collision_markers_;
  }

  inline bool areCollisionsVisible() const
  {
    return collisions_visible_;
  }

  inline void setCollisionsVisible(bool shown)
  {
    collisions_visible_ = shown;
  }

  inline void showCollisions()
  {
    setCollisionsVisible(true);
  }

  inline void hideCollisions()
  {
    setCollisionsVisible(false);
  }

  inline size_t getTrajectorySize() const
  {
    return trajectory_.points.size();
  }

  inline bool shouldRefreshColors() const
  {
    return should_refresh_colors_;
  }

  inline bool hasRefreshedColors() const
  {
    return has_refreshed_colors_;
  }

  inline void setHasRefreshedColors(bool refresh)
  {
    has_refreshed_colors_ = refresh;

    if(refresh)
    {
      should_refresh_colors_ = false;
    }
  }

  inline void refreshColors()
  {
    should_refresh_colors_ = true;
    has_refreshed_colors_ = false;
    refresh_timer_ = ros::Duration(0.0);
  }

  inline RenderType getRenderType() const
  {
    return render_type_;
  }

  inline void setRenderType(RenderType renderType)
  {
    render_type_ = renderType;
  }

  inline TrajectoryRenderType getTrajectoryRenderType() const
  {
    return trajectory_render_type_;
  }

  inline void setTrajectoryRenderType(TrajectoryRenderType renderType)
  {
    trajectory_render_type_ = renderType;
  }

  inline void reset()
  {

    if(current_state_ != NULL)
    {
      delete current_state_;
      current_state_ = NULL;
    }

    is_playing_ = false;
    is_visible_ = false;
    current_trajectory_point_ = 0;
    state_changed_ = false;
  }

  inline planning_models::KinematicState* getCurrentState()
  {
    return current_state_;
  }

  inline void setCurrentState(planning_models::KinematicState* state)
  {
    current_state_ = state;
    state_changed_ = true;
  }

  inline void setMotionPlanRequestId(const unsigned int& Id)
  {
    motion_plan_request_Id_ = Id;
  }

  inline const unsigned int& getMotionPlanRequestId() const
  {
    return motion_plan_request_Id_;
  }

  inline void setPlanningSceneId(const unsigned int id) {
    planning_scene_id_ = id;
  }

  unsigned int getPlanningScendId() const {
    return planning_scene_id_;
  }

  inline void setCurrentPoint(unsigned int point)
  {
    current_trajectory_point_ = point;
    state_changed_ = true;
  }

  inline unsigned int getCurrentPoint() const
  {
    return current_trajectory_point_;
  }

  inline unsigned int getBadPoint() const
  {
    return trajectory_bad_point_;
  }

  inline void setGroupname(const std::string& group_name)
  {
    group_name_ = group_name;
  }

  inline bool isPlaying() const
  {
    return is_playing_;
  }

  inline void play()
  {
    is_playing_ = true;
    playback_start_time_ = ros::Time::now();
  }

  inline void stop()
  {
    is_playing_ = false;
  }

  inline bool isVisible() const
  {
    return is_visible_;
  }

  inline void setVisible(bool visible)
  {
    is_visible_ = visible;
  }

  inline MarkerType getMarkerType() const 
  {
    return marker_type_;
  }

  inline void setMarkerType(MarkerType mt) 
  {
    marker_type_ = mt;
  }

  inline void show()
  {
    setVisible(true);
  }

  inline void hide()
  {
    setVisible(false);
  }

  inline const ros::Duration& getDuration() const
  {
    return duration_;
  }

  inline void setDuration(const ros::Duration& duration)
  {
    duration_ = duration;
  }

  inline const std_msgs::ColorRGBA& getColor() const
  {
    return color_;
  }

  inline void setColor(const std_msgs::ColorRGBA& color)
  {
    color_ = color;
  }

  inline std::string getSource()
  {
    return source_;
  }

  inline trajectory_msgs::JointTrajectory& getTrajectory()
  {
    return trajectory_;
  }

  inline void setSource(const std::string& source)
  {
    source_ = source;
  }

  inline void setTrajectory(const trajectory_msgs::JointTrajectory& trajectory)
  {
    trajectory_ = trajectory;
  }

  inline trajectory_msgs::JointTrajectory& getTrajectoryError()
  {
    return trajectory_error_;
  }

  inline void setTrajectoryError(const trajectory_msgs::JointTrajectory& trajectory_error)
  {
    trajectory_error_ = trajectory_error;
  }

  inline unsigned int getId() const
  {
    return id_;
  }

  inline void setId(const unsigned int& id)
  {
    id_ = id;
    name_ = getTrajectoryNameFromId(id_);
  }

  const std::string& getName() const {
    return name_;
  }

  inline const std::string& getGroupName() const
  {
    return group_name_;
  }

  inline void setBadPoint(unsigned int point)
  {
    trajectory_bad_point_ = point;
  }

  inline void setGroupName(std::string name)
  {
    group_name_ = name;
  }

  void updateCollisionMarkers(planning_environment::CollisionModels* cm_, MotionPlanRequestData& motionPlanRequest,
                              ros::ServiceClient* distance_state_validity_service_client_);
};

struct PlanningSceneParameters
{
  std::string left_ik_name_;
  std::string right_ik_name_;
  std::string non_coll_left_ik_name_;
  std::string non_coll_right_ik_name_;
  std::string left_interpolate_service_name_;
  std::string right_interpolate_service_name_;
  std::string planner_1_service_name_;
  std::string planner_2_service_name_;
  std::string proximity_space_service_name_;
  std::string proximity_space_validity_name_;
  std::string set_planning_scene_diff_name_;
  std::string trajectory_filter_1_service_name_;
  std::string trajectory_filter_2_service_name_;
  std::string proximity_space_planner_name_;
  std::string vis_topic_name_;
  std::string right_ik_link_;
  std::string left_ik_link_;
  std::string left_redundancy_;
  std::string right_redundancy_;
  std::string right_arm_group_;
  std::string left_arm_group_;
  std::string execute_left_trajectory_;
  std::string execute_right_trajectory_;
  std::string list_controllers_service_;
  std::string unload_controllers_service_;
  std::string load_controllers_service_;
  std::string switch_controllers_service_;
  std::string gazebo_model_name_;
  std::string robot_description_param_;
  bool use_robot_data_;
  bool sync_robot_state_with_gazebo_;
};

class PlanningSceneEditor
{
public:
  enum GeneratedShape
    {
      Box, Cylinder, Sphere
    };
protected:

  enum MonitorStatus
    {
      idle, Executing, WaitingForStop, Done
    };

  struct StateRegistry
  {
    planning_models::KinematicState* state;
    std::string source;
  };

  struct SelectableObject
  {
    SelectableObject() {
      attach_ = false;
      detach_ = false;
      attached_collision_object_.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
      collision_object_.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
    }

    bool attach_;
    bool detach_;
    arm_navigation_msgs::AttachedCollisionObject attached_collision_object_;
    arm_navigation_msgs::CollisionObject collision_object_;
    visualization_msgs::InteractiveMarker selection_marker_;
    visualization_msgs::InteractiveMarker control_marker_;
    std_msgs::ColorRGBA color_;

    std::string id_;
  };

  struct IKController
  {
    unsigned int motion_plan_id_;
    visualization_msgs::InteractiveMarker start_controller_;
    visualization_msgs::InteractiveMarker end_controller_;
  };

  virtual void updateState()
  {
  }

  virtual void planCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0;

  virtual void filterCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode) = 0;

  virtual void attachObjectCallback(const std::string& name) = 0;
  // virtual void detachObjectCallback(arm_navigation_msgs::CollisionObject& object) = 0;

  void changeToAttached(const std::string& name);

  virtual void selectedTrajectoryCurrentPointChanged( unsigned int new_current_point ) {}

  boost::recursive_mutex lock_scene_;
  arm_navigation_msgs::ArmNavigationErrorCodes last_collision_set_error_code_;
  move_arm_warehouse::MoveArmWarehouseLoggerReader* move_arm_warehouse_logger_reader_;
  planning_environment::CollisionModels* cm_;
  planning_models::KinematicState* robot_state_;
  PlanningSceneParameters params_;
  ros::NodeHandle nh_;
  ros::Publisher clock_publisher_;
  ros::Publisher joint_state_publisher_;
  ros::Publisher vis_marker_array_publisher_;
  ros::Publisher vis_marker_publisher_;
  ros::Subscriber joint_state_subscriber_;
  ros::Subscriber r_arm_controller_state_subscriber_;
  ros::Subscriber l_arm_controller_state_subscriber_;
  ros::ServiceClient collision_proximity_planner_client_;
  ros::ServiceClient distance_aware_service_client_;
  ros::ServiceClient distance_state_validity_service_client_;
  ros::ServiceClient set_planning_scene_diff_client_;
  ros::ServiceClient left_ik_service_client_;
  ros::ServiceClient left_interpolate_service_client_;
  ros::ServiceClient non_coll_left_ik_service_client_;
  ros::ServiceClient non_coll_right_ik_service_client_;
  ros::ServiceClient planning_1_service_client_;
  ros::ServiceClient planning_2_service_client_;
  ros::ServiceClient right_ik_service_client_;
  ros::ServiceClient right_interpolate_service_client_;
  ros::ServiceClient trajectory_filter_1_service_client_;
  ros::ServiceClient trajectory_filter_2_service_client_;
  ros::ServiceClient gazebo_joint_state_client_;
  ros::ServiceClient list_controllers_client_;
  ros::ServiceClient load_controllers_client_;
  ros::ServiceClient unload_controllers_client_;
  ros::ServiceClient switch_controllers_client_;
  ros::ServiceClient pause_gazebo_client_;
  ros::ServiceClient unpause_gazebo_client_;
  ros::ServiceClient set_link_properties_client_;
  ros::ServiceClient get_link_properties_client_;

  std::map<std::string, double> robot_state_joint_values_;
  std::vector<ros::Time> last_creation_time_query_;
  tf::TransformBroadcaster transform_broadcaster_;
  tf::TransformListener transform_listener_;
  std::map<std::string, actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>*> arm_controller_map_;
  unsigned int max_trajectory_id_;
  unsigned int max_collision_object_id_;

  bool warehouse_data_loaded_once_;
  int  active_planner_index_;
  bool use_primary_filter_;

  trajectory_msgs::JointTrajectory logged_trajectory_;
  trajectory_msgs::JointTrajectory logged_trajectory_controller_error_;
  ros::Time logged_trajectory_start_time_;
  int logged_trajectory_controller_error_code_;

  bool send_collision_markers_;
  std::string collision_marker_state_;
  visualization_msgs::MarkerArray collision_markers_;
  planning_models::KinematicState* paused_collision_state_;
  std_msgs::ColorRGBA point_color_;
  std::vector<geometry_msgs::TransformStamped> robot_transforms_;

  interactive_markers::MenuHandler::FeedbackCallback attached_collision_object_feedback_ptr_;
  interactive_markers::MenuHandler::FeedbackCallback collision_object_selection_feedback_ptr_;
  interactive_markers::MenuHandler::FeedbackCallback collision_object_movement_feedback_ptr_;
  interactive_markers::MenuHandler::FeedbackCallback ik_control_feedback_ptr_;
  interactive_markers::MenuHandler::FeedbackCallback joint_control_feedback_ptr_;

  interactive_markers::InteractiveMarkerServer* interactive_marker_server_;

  std::map<std::string, SelectableObject>* selectable_objects_;
  std::map<std::string, IKController>* ik_controllers_;

  std::string current_planning_scene_name_;
  std::string selected_motion_plan_name_;
  std::string selected_trajectory_name_;
  std::string logged_group_name_;
  std::string logged_motion_plan_request_;
  std::map<string, MenuEntryMap> menu_entry_maps_;
  MenuHandlerMap menu_handler_map_;

  std::map<string, ros::ServiceClient*>* collision_aware_ik_services_;
  std::map<string, ros::ServiceClient*>* non_collision_aware_ik_services_;
  std::map<string, ros::ServiceClient*>* interpolated_ik_services_;
  std::map<string, arm_navigation_msgs::ArmNavigationErrorCodes> error_map_;
  std::vector<StateRegistry> states_;

  interactive_markers::MenuHandler::EntryHandle last_resize_handle_;

  std_msgs::ColorRGBA last_collision_object_color_;
  std_msgs::ColorRGBA last_mesh_object_color_;

  MonitorStatus monitor_status_;
  ros::Time time_of_controller_done_callback_;
  ros::Time time_of_last_moving_notification_;

  ros::Time last_marker_start_time_;
  ros::Duration marker_dt_;

  void attachCollisionObject(const std::string& name, 
                             const std::string& link_name,
                             const std::vector<std::string>& touch_links);
  
  void createSelectableMarkerFromCollisionObject(arm_navigation_msgs::CollisionObject& object, std::string name,
                                                 std::string description, std_msgs::ColorRGBA color, bool insert_selectable=true);

  
  
public:

  std::map<std::string, PlanningSceneData> planning_scene_map_;

  std::map<std::string, std::map<std::string, TrajectoryData> > trajectory_map_;

  std::map<std::string, MotionPlanRequestData> motion_plan_map_;

  std::map<std::string, bool> joint_clicked_map_;

  std::map<std::string, tf::Transform> joint_prev_transform_map_;

  PlanningSceneEditor();
  PlanningSceneEditor(PlanningSceneParameters& params);
  ~PlanningSceneEditor();

  bool filterTrajectory(MotionPlanRequestData& requestData, TrajectoryData& trajectory, unsigned int& filter_id);

  bool getAllAssociatedMotionPlanRequests(const unsigned int id, 
                                          std::vector<unsigned int>& ids,
                                          std::vector<std::string>& stages,
                                          std::vector<arm_navigation_msgs::MotionPlanRequest>& requests);

  bool getAllAssociatedPausedStates(const unsigned int id, 
                                    std::vector<ros::Time>& paused_times);

  bool getAllAssociatedTrajectorySources(const unsigned int planning_id,
                                         const unsigned int mpr_id,
                                         std::vector<unsigned int>& trajectory_ids,
                                         std::vector<std::string>& trajectory_sources);

  bool getAllPlanningSceneTimes(std::vector<ros::Time>& planning_scene_times,
                                vector<unsigned int>& planning_scene_ids);


  bool getMotionPlanRequest(const ros::Time& time, const std::string& stage,
                            arm_navigation_msgs::MotionPlanRequest& mpr, std::string& id,
                            std::string& planning_scene_id);

  bool getPausedState(const unsigned int id, 
                      const ros::Time& paused_time,
                      head_monitor_msgs::HeadMonitorFeedback& paused_state);

  bool getPlanningSceneOutcomes(const unsigned int id, 
                                std::vector<std::string>& pipeline_stages,
                                std::vector<arm_navigation_msgs::ArmNavigationErrorCodes>& error_codes,
                                std::map<std::string, arm_navigation_msgs::ArmNavigationErrorCodes>& error_map);


  bool loadPlanningScene(const ros::Time& time, 
                         const unsigned int id);

  bool planToKinematicState(const planning_models::KinematicState& state, const std::string& group_name,
                            const std::string& end_effector_name, unsigned int& trajectoryid_Out,
                            unsigned int& planning_scene_id);

  bool planToRequest(MotionPlanRequestData& data, unsigned int& trajectoryid_Out);

  bool planToRequest(const std::string& requestid, unsigned int& trajectoryid_Out);

  bool playTrajectory(MotionPlanRequestData& requestData, TrajectoryData& data);

  bool sendPlanningScene(PlanningSceneData& data);

  bool solveIKForEndEffectorPose(MotionPlanRequestData& mpr, PositionType type, bool coll_aware = true,
                                 double change_redundancy = 0.0);


  interactive_markers::MenuHandler::EntryHandle registerMenuEntry(std::string menu, std::string entryName,
                                                                  interactive_markers::MenuHandler::FeedbackCallback& callback);



  interactive_markers::MenuHandler::EntryHandle registerSubMenuEntry(std::string menu, std::string name,
                                                                     std::string subMenu,
                                                                     interactive_markers::MenuHandler::FeedbackCallback& callback);

  std::string createNewPlanningScene();

  virtual void onPlanningSceneLoaded(int scene, int numScenes)
  {
  }

  void collisionObjectMovementCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);

  void collisionObjectSelectionCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);

  void attachedCollisionObjectInteractiveCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);

  std::string createCollisionObject(const std::string& name, geometry_msgs::Pose pose, GeneratedShape shape, float scaleX, float scaleY,
                                    float scaleZ, std_msgs::ColorRGBA color,
                                    bool selectable);

  std::string createMeshObject(const std::string& name, 
                               geometry_msgs::Pose pose,
                               const std::string& filename,
                               const tf::Vector3& scale,
                               std_msgs::ColorRGBA color);
  void createIKController(MotionPlanRequestData& data, PositionType type, bool rePose = true);

  void createIkControllersFromMotionPlanRequest(MotionPlanRequestData& data, bool rePose = true);

  void createJointMarkers(MotionPlanRequestData& data, planning_scene_utils::PositionType position);


  void createMotionPlanRequest(const planning_models::KinematicState& start_state,
                               const planning_models::KinematicState& end_state, 
                               const std::string& group_name,
                               const std::string& end_effector_name, 
                               const unsigned int& planning_scene_name,
                               const bool fromRobotState, 
                               unsigned int& motionPlan_id_Out);


  void initMotionPlanRequestData(const unsigned int& planning_scene_id, 
                                 const std::vector<unsigned int>& ids,
                                 const std::vector<std::string>& stages,
                                 const std::vector<arm_navigation_msgs::MotionPlanRequest>& requests);


  void deleteJointMarkers(MotionPlanRequestData& data, PositionType type);

  void deleteKinematicStates();

  void deleteMotionPlanRequest(const unsigned int& id,
                               std::vector<unsigned int>& erased_trajectories);

  void deleteTrajectory(unsigned int mpr_id, unsigned int traj_id);


  // void determineOrientationConstraintsGivenState(const MotionPlanRequestData& mpr,
  //                                                const planning_models::KinematicState& state,
  //                                                arm_navigation_msgs::OrientationConstraint& goal_constraint,
  //                                                arm_navigation_msgs::OrientationConstraint& path_constraint);

  void executeTrajectory(const std::string& mpr_name, const std::string& traj_name);

  void executeTrajectory(TrajectoryData& data);

  void getAllRobotStampedTransforms(const planning_models::KinematicState& state,
                                    vector<geometry_msgs::TransformStamped>& trans_vector, const ros::Time& stamp);

  void getMotionPlanningMarkers(visualization_msgs::MarkerArray& arr);

  void getTrajectoryMarkers(visualization_msgs::MarkerArray& arr);

  void IKControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);

  void JointControllerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);

  void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
                              const control_msgs::FollowJointTrajectoryResultConstPtr& result);

  void armHasStoppedMoving();

  void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);

void jointTrajectoryControllerStateCallback(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr& joint_state);


  void loadAllWarehouseData();

  void makeInteractive1DOFRotationMarker(tf::Transform transform, tf::Vector3 axis, string name, string description,
                                         float scale = 1.0f, float angle = 0.0f);

  void makeInteractive1DOFTranslationMarker(tf::Transform transform, tf::Vector3 axis, string name, string description,
                                            float scale = 1.0f, float value = 0.0f);

  void printTrajectoryPoint(const std::vector<std::string>& joint_names, const std::vector<double>& joint_values);

  void randomlyPerturb(MotionPlanRequestData& mpr, PositionType type);

  void savePlanningScene(PlanningSceneData& data, bool copy = false);

  void sendMarkers();

  void sendTransformsAndClock();

  void makeSelectableAttachedObjectFromPlanningScene(const arm_navigation_msgs::PlanningScene& scene,
                                                     arm_navigation_msgs::AttachedCollisionObject& att);

  void setCurrentPlanningScene(std::string id, bool loadRequests = true, bool loadTrajectories = true);

  void setIKControlsVisible(std::string id, PositionType type, bool visible);

  void setJointState(MotionPlanRequestData& data, PositionType position, std::string& jointName, tf::Transform value);

  void updateJointStates();

  bool hasTrajectory(const std::string& mpr_name,
                     const std::string& traj_name) const;

  inline std::string generateNewCollisionObjectId()
  {
    std::stringstream stream;
    stream << "collision_object_";
    max_collision_object_id_++;
    stream << max_collision_object_id_;
    return stream.str();
  }

  inline unsigned int generateNewPlanningSceneId()
  {
    return move_arm_warehouse_logger_reader_->determineNextPlanningSceneId();
  }


  void deleteCollisionObject(std::string& name);


  inline void lockScene()
  {
    lock_scene_.lock();
  }

  inline void unlockScene()
  {
    lock_scene_.unlock();
  }

  inline planning_environment::CollisionModels* getCollisionModel()
  {
    return cm_;
  }

  inline void setCollisionModel(planning_environment::CollisionModels* model, bool shouldDelete = false)
  {
    if(shouldDelete && cm_ != NULL)
    {
      delete cm_;
      cm_ = model;
    }
    cm_ = model;
  }

  inline planning_models::KinematicState* getRobotState()
  {
    return robot_state_;
  }

  inline void setRobotState(planning_models::KinematicState* robot_state, bool shouldDelete = true)
  {
    if(shouldDelete && robot_state_ != NULL)
    {
      delete robot_state_;
      robot_state_ = NULL;
    }

    robot_state_ = robot_state;
  }

  inline move_arm_warehouse::MoveArmWarehouseLoggerReader* getLoggerReader()
  {
    return move_arm_warehouse_logger_reader_;
  }

  inline void setLoggerReader(move_arm_warehouse::MoveArmWarehouseLoggerReader* loggerReader,
                              bool shouldDelete = true)
  {
    if(move_arm_warehouse_logger_reader_ != NULL)
    {
      delete move_arm_warehouse_logger_reader_;
      move_arm_warehouse_logger_reader_ = NULL;
    }

    move_arm_warehouse_logger_reader_ = loggerReader;
  }
};

}
#endif