arm_manip_node.h

Go to the documentation of this file.

/******************************************************************************
 * \file but_arm_manipulation_node.h
 * \brief Definition of ManualArmManipActionServer and CArmManipulationEditor classes.
 * \author Zdenek Materna (imaterna@fit.vutbr.cz)
 *
 * $Id:$
 *
 * Copyright (C) Brno University of Technology
 *
 * This file is part of software developed by dcgm-robotics@FIT group.
 *
 * Author: Zdenek Materna (imaterna@fit.vutbr.cz)
 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
 * Date: dd/mm/2012
 * 
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This file is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once
#ifndef BUT_ARMNAVIGATION_NODE_H
#define BUT_ARMNAVIGATION_NODE_H


#include <ros/ros.h>
#include "srs_assisted_arm_navigation/move_arm_utils/move_arm_utils.h"
#include <assert.h>
#include <unistd.h>
#include <time.h>
#include <boost/circular_buffer.hpp>
#include <boost/thread.hpp>
#include <geometry_msgs/Vector3.h>
#include "math.h"

#include "srs_assisted_arm_navigation_msgs/ArmNavNew.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavPlan.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavPlay.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavReset.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavRefresh.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavExecute.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavStart.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavCollObj.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavSetAttached.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavMovePalmLink.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavMovePalmLinkRel.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavSwitchAttCO.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavStep.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavStop.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavSuccess.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavFailed.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavRepeat.h"
#include "srs_assisted_arm_navigation_msgs/ArmNavRemoveCollObjects.h"
#include "srs_assisted_arm_navigation_msgs/AssistedArmNavigationState.h"

#include "srs_interaction_primitives/ClickablePositions.h"
#include "srs_interaction_primitives/PositionClicked.h"

#include "srs_assisted_arm_navigation_msgs/ManualArmManipAction.h"

#include <actionlib/server/simple_action_server.h>

#include "srs_assisted_arm_navigation/services_list.h"
#include "srs_assisted_arm_navigation/topics_list.h"

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Int32MultiArray.h"
#include "sensor_msgs/Joy.h"
#include "std_srvs/Empty.h"

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
//#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include "geometry_msgs/Vector3.h"
#include <tf/transform_broadcaster.h>

namespace srs_assisted_arm_navigation {

static const std::string VIS_TOPIC_NAME = "planning_scene_visualizer_markers";
static const std::string EXECUTE_RIGHT_TRAJECTORY = "none";
static const std::string EXECUTE_LEFT_TRAJECTORY = "/arm_controller/follow_joint_trajectory";
static const std::string LEFT_IK_NAME = "/cob3_arm_kinematics/get_constraint_aware_ik";
static const std::string RIGHT_IK_NAME = "none";
static const std::string NON_COLL_LEFT_IK_NAME = "/cob3_arm_kinematics/get_ik";
static const std::string NON_COLL_RIGHT_IK_NAME = "none";
static const std::string RIGHT_ARM_GROUP = "none";
static const std::string LEFT_ARM_GROUP = "arm";
static const std::string RIGHT_ARM_REDUNDANCY = "none";
static const std::string LEFT_ARM_REDUNDANCY = "none";
static const std::string LEFT_IK_LINK = "arm_7_link";
static const std::string RIGHT_IK_LINK = "none";
static const std::string PLANNER_1_SERVICE_NAME = "/ompl_planning/plan_kinematic_path";
static const std::string PLANNER_2_SERVICE_NAME = "none";
static const std::string LEFT_INTERPOLATE_SERVICE_NAME = "none";
static const std::string RIGHT_INTERPOLATE_SERVICE_NAME = "none";
static const std::string TRAJECTORY_FILTER_1_SERVICE_NAME = "/trajectory_filter_server/filter_trajectory_with_constraints";
static const std::string TRAJECTORY_FILTER_2_SERVICE_NAME = "none";
static const std::string PROXIMITY_SPACE_SERVICE_NAME = "none";
static const std::string PROXIMITY_SPACE_VALIDITY_NAME = "none";
static const std::string PROXIMITY_SPACE_PLANNER_NAME = "none";
static const std::string LIST_CONTROLLERS_SERVICE = "/pr2_controller_manager/list_controllers";
static const std::string LOAD_CONTROLLERS_SERVICE = "/pr2_controller_manager/load_controller";
static const std::string UNLOAD_CONTROLLERS_SERVICE = "/pr2_controller_manager/unload_controller";
static const std::string SWITCH_CONTROLLERS_SERVICE = "/pr2_controller_manager/switch_controller";
static const std::string GAZEBO_ROBOT_MODEL = "robot";
static const std::string ROBOT_DESCRIPTION_PARAM = "robot_description";
static const ros::Duration PLANNING_DURATION = ros::Duration(5.0);
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "environment_server/set_planning_scene_diff";

static const std::string WORLD_FRAME = "/map";



static const double START_TIMEOUT = 60.0; 
static const double SOLVE_TIMEOUT = (5*60.0); 
static const double INFLATE_BB = 1.0;

class ManualArmManipActionServer{

public:

  ManualArmManipActionServer(std::string name):
    server_(nh_, name, boost::bind(&ManualArmManipActionServer::executeCB, this, _1), false), action_name_(name) {

    inited_ = false;
    state_ = S_NONE;

    start_timeout_ = ros::Duration(START_TIMEOUT);
    solve_timeout_ = ros::Duration(SOLVE_TIMEOUT);

    server_.start(); 
  }

  ~ManualArmManipActionServer(void) {};


  enum {S_NONE,S_NEW,S_PLAN,S_EXECUTE,S_RESET,S_SUCCESS,S_FAILED, S_REPEAT};

  unsigned int get_state() {

          return state_;

  }

  void srv_set_state(unsigned int n);

  ros::Duration start_timeout_; 
  ros::Duration solve_timeout_; 
  protected:

    unsigned int state_;             
    unsigned int new_state_;         
    ros::NodeHandle nh_;


    actionlib::SimpleActionServer<srs_assisted_arm_navigation_msgs::ManualArmManipAction> server_;
    std::string action_name_;
    srs_assisted_arm_navigation_msgs::ManualArmManipActionFeedback feedback_;
    srs_assisted_arm_navigation_msgs::ManualArmManipActionResult result_;

    ros::Time start_time_; 
    ros::Time timeout_; 
    bool inited_; 
    planning_scene_utils::PlanningSceneParameters params_;



    void setFeedbackFalse();

    void executeCB(const srs_assisted_arm_navigation_msgs::ManualArmManipGoalConstPtr &goal);


  private:


};


typedef struct {

  std::string name;
  std::string id;
  geometry_msgs::PoseStamped pose;
  geometry_msgs::Point bb_lwh;
  bool allow_collision;
  bool attached;
  bool allow_pregrasps;

  std::string topic_name;

} t_det_obj;

class CArmManipulationEditor : public planning_scene_utils::PlanningSceneEditor
{

public:

   CArmManipulationEditor(planning_scene_utils::PlanningSceneParameters& params,  std::vector<string> clist);
  ~CArmManipulationEditor();

  virtual void onPlanningSceneLoaded();
  virtual void updateState();

  virtual void planCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode);
  virtual void filterCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode);

  virtual void attachObjectCallback(const std::string& name);
  virtual void selectedTrajectoryCurrentPointChanged( unsigned int new_current_point );

  void createAttachedObj();

  bool ArmNavNew(srs_assisted_arm_navigation_msgs::ArmNavNew::Request &req, srs_assisted_arm_navigation_msgs::ArmNavNew::Response &res);
  bool ArmNavPlan(srs_assisted_arm_navigation_msgs::ArmNavPlan::Request &req, srs_assisted_arm_navigation_msgs::ArmNavPlan::Response &res);
  bool ArmNavPlay(srs_assisted_arm_navigation_msgs::ArmNavPlay::Request &req, srs_assisted_arm_navigation_msgs::ArmNavPlay::Response &res);
  bool ArmNavExecute(srs_assisted_arm_navigation_msgs::ArmNavExecute::Request &req, srs_assisted_arm_navigation_msgs::ArmNavExecute::Response &res);
  bool ArmNavReset(srs_assisted_arm_navigation_msgs::ArmNavReset::Request &req, srs_assisted_arm_navigation_msgs::ArmNavReset::Response &res);
  bool ArmNavRefresh(srs_assisted_arm_navigation_msgs::ArmNavRefresh::Request &req, srs_assisted_arm_navigation_msgs::ArmNavRefresh::Response &res);

  bool ArmNavSuccess(srs_assisted_arm_navigation_msgs::ArmNavSuccess::Request &req, srs_assisted_arm_navigation_msgs::ArmNavSuccess::Response &res);
  bool ArmNavFailed(srs_assisted_arm_navigation_msgs::ArmNavFailed::Request &req, srs_assisted_arm_navigation_msgs::ArmNavFailed::Response &res);
  bool ArmNavRepeat(srs_assisted_arm_navigation_msgs::ArmNavRepeat::Request &req, srs_assisted_arm_navigation_msgs::ArmNavRepeat::Response &res);

  bool ArmNavCollObj(srs_assisted_arm_navigation_msgs::ArmNavCollObj::Request &req, srs_assisted_arm_navigation_msgs::ArmNavCollObj::Response &res);
  bool ArmNavSetAttached(srs_assisted_arm_navigation_msgs::ArmNavSetAttached::Request &req, srs_assisted_arm_navigation_msgs::ArmNavSetAttached::Response &res);
  bool ArmNavMovePalmLink(srs_assisted_arm_navigation_msgs::ArmNavMovePalmLink::Request &req, srs_assisted_arm_navigation_msgs::ArmNavMovePalmLink::Response &res);
  bool ArmNavMovePalmLinkRel(srs_assisted_arm_navigation_msgs::ArmNavMovePalmLinkRel::Request &req, srs_assisted_arm_navigation_msgs::ArmNavMovePalmLinkRel::Response &res);
  bool ArmNavSwitchACO(srs_assisted_arm_navigation_msgs::ArmNavSwitchAttCO::Request &req, srs_assisted_arm_navigation_msgs::ArmNavSwitchAttCO::Response &res);
  bool ArmNavStep(srs_assisted_arm_navigation_msgs::ArmNavStep::Request &req, srs_assisted_arm_navigation_msgs::ArmNavStep::Response &res);
  bool ArmNavRemoveCollObjects(srs_assisted_arm_navigation_msgs::ArmNavRemoveCollObjects::Request &req, srs_assisted_arm_navigation_msgs::ArmNavRemoveCollObjects::Response &res);
  
  bool ArmNavStop(srs_assisted_arm_navigation_msgs::ArmNavStop::Request &req, srs_assisted_arm_navigation_msgs::ArmNavStop::Response &res);


  void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);

  typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::Vector3, geometry_msgs::Vector3> MySyncPolicy;

  ros::Subscriber offset_sub_;
  ros::Subscriber rot_offset_sub_;
  ros::Subscriber joy_sub_;

  void spacenavOffsetCallback(const geometry_msgs::Vector3ConstPtr& offset);
  void spacenavRotOffsetCallback(const geometry_msgs::Vector3ConstPtr& rot_offset);



  //ros::Subscriber joy_sub_;

  void spin_callback(const ros::TimerEvent&);

  void reset();

  void remove_coll_objects();

  bool refresh();

  ManualArmManipActionServer * action_server_ptr_;
  tf::TransformListener *tfl_;

  double inflate_bb_;

  bool aco_;

  ros::Publisher gripper_rpy_publisher_;

  std::string world_frame_;

  bool transf(std::string target_frame, geometry_msgs::PoseStamped& pose);

protected:

  ros::Subscriber sub_click_;

  void subClick(const srs_interaction_primitives::PositionClickedConstPtr &msg);

  void tfTimerCallback(const ros::TimerEvent& ev);

  tf::TransformBroadcaster br_;

  void timerCallback(const ros::TimerEvent& ev);

  void spacenavCallback(const ros::TimerEvent& ev);

  ros::Timer tf_timer_;

  ros::Timer state_timer_;

  ros::Timer spacenav_timer_;

  bool use_spacenav_;

  struct {

          boost::mutex mutex_;

          bool offset_received_;
          bool rot_offset_received_;

          bool lock_position_;
          bool lock_orientation_;

          geometry_msgs::Vector3 offset;
          geometry_msgs::Vector3 rot_offset;

          double max_val_;
          double min_val_th_;
          double step_;
          double rot_step_;
          bool use_rviz_cam_;
          std::string rviz_cam_link_;

          vector<int32_t> buttons_;


  } spacenav;

  ros::Publisher arm_nav_state_pub_;

  void normAngle(double& a);

  void processSpaceNav();


  std::string planning_scene_id; 
  unsigned int mpr_id; 
  unsigned int traj_id; 
  unsigned int filt_traj_id; 
  std::vector<std::string> coll_obj_attached_id; 
  //std::vector<std::string> coll_obj_bb_id;
  bool inited; 
  bool planned_;

  std::vector<std::string> links_; 
  boost::circular_buffer<geometry_msgs::Pose> * gripper_poses_;

  boost::thread gripper_poses_thread_;

  bool disable_gripper_poses_;

  void GripperPoses();
  void GripperPosesClean();

  geometry_msgs::Vector3 GetAsEuler(geometry_msgs::Quaternion quat);

  bool step_used_;

  //boost::mutex im_server_mutex_;

  std::string add_coll_obj_attached(double x, double y, double z, double scx, double scz);
  //std::string add_coll_obj_bb(std::string name, geometry_msgs::PoseStamped pose, geometry_msgs::Point bb_lwh, bool coll, bool attached);
  std::string add_coll_obj_bb(t_det_obj &obj);

  std::string collision_objects_frame_id_;
  bool coll_objects_selectable_;

  //void armHasStoppedMoving();


  std::vector<t_det_obj> coll_obj_det;

  bool findIK(geometry_msgs::Pose new_pose);

  bool checkPose(geometry_msgs::PoseStamped &p, std::string frame);

  bool joint_controls_;
  std::string aco_link_;

  std::string end_eff_link_;
  //bool aco_state_;

  struct {

          arm_navigation_msgs::ArmNavigationErrorCodes plan_error_code;
          arm_navigation_msgs::ArmNavigationErrorCodes filter_error_code;

          std::string plan_desc;
          std::string filter_desc;

          bool out_of_limits;

  } arm_nav_state_;




private:

};

}

#endif