sbpl_arm_planner_node.h

Go to the documentation of this file.

/*
 * Copyright (c) 2009, 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, Inc. 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.
 */

#ifndef __SBPL_ARM_PLANNER_NODE_H_
#define __SBPL_ARM_PLANNER_NODE_H_

#include <iostream>
#include <map>
#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <message_filters/subscriber.h>
#include <tf/message_filter.h>
#include <tf/transform_datatypes.h>
#include <kdl/chain.hpp>
#include <kdl/frames.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <angles/angles.h>
#include <sbpl_arm_planner_node/visualize_arm.h>

#include <geometry_msgs/Pose.h>
#include <mapping_msgs/CollisionMap.h>
#include <mapping_msgs/AttachedCollisionObject.h>
#include <motion_planning_msgs/GetMotionPlan.h>
#include <trajectory_msgs/JointTrajectoryPoint.h> 
#include <kinematics_msgs/GetPositionIK.h>
#include <kinematics_msgs/GetPositionFK.h>

namespace sbpl_arm_planner
{
  class SBPLArmPlannerNode
  {
    public:
      SBPLArmPlannerNode();

      ~SBPLArmPlannerNode();

      bool init();

      int run();

      bool planKinematicPath(motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res);

    private:

      ros::NodeHandle node_handle_, root_handle_;
      ros::ServiceServer planning_service_;
      ros::Subscriber object_subscriber_;
      message_filters::Subscriber<mapping_msgs::CollisionMap> collision_map_subscriber_;
      tf::MessageFilter<mapping_msgs::CollisionMap> *collision_map_filter_;

      //remove
      ros::Subscriber collision_object_subscriber_;
      void collisionObjectCallback(const mapping_msgs::CollisionObjectConstPtr &collision_object);
      std::vector<std::vector<double> > col_objects_;

      KDL::ChainFkSolverPos_recursive *jnt_to_pose_solver_;

      std::string fk_service_name_;
      
      std::string ik_service_name_;

      std::string side_;

      double allocated_time_;

      double waypoint_time_;

      double env_resolution_;

      /* params */
      bool forward_search_;
      bool search_mode_;
      bool visualize_expanded_states_;
      bool visualize_heuristic_;
      bool visualize_goal_;
      bool planner_initialized_;
      bool print_path_;
      bool visualize_trajectory_;
      bool visualize_collision_model_trajectory_;
      bool use_research_heuristic_;
      bool use_first_solution_;
      bool attached_object_;
      int throttle_;
      int num_joints_;

      std::string collision_map_topic_;
      std::string robot_description_;
      std::string node_name_;
      std::string reference_frame_;
      std::string map_frame_;
      std::string planning_joint_;
      std::string arm_name_;
      std::string trajectory_type_;
      std::string arm_description_filename_;
      std::string mprims_filename_;
      std::string attached_object_frame_;
      std::vector<std::string> joint_names_;
      std::vector<std::vector<double> > dpath_;
      std::map<std::string, mapping_msgs::CollisionObject> object_map_;

      /* planner & environment */
      MDPConfig mdp_cfg_;
      EnvironmentROBARM3D sbpl_arm_env_;
      SBPLPlanner *planner_;
      SBPLCollisionSpace* cspace_;
      OccupancyGrid* grid_;
      VisualizeArm* aviz_;
      
      boost::mutex colmap_mutex_;
      boost::mutex object_mutex_;

      /* transforms and kinematics */
      tf::TransformListener tf_;
      tf::StampedTransform transform_;
      KDL::Chain arm_chain_;
      KDL::JntArray jnt_pos_;
      KDL::Frame kdl_transform_;

      bool initializePlannerAndEnvironment();

      bool setStart(const sensor_msgs::JointState &start_state);

      bool setGoalPosition(const motion_planning_msgs::Constraints &goals);

      void updateMapFromCollisionMap(const mapping_msgs::CollisionMapConstPtr &collision_map);

      void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collision_map);

      void attachedObjectCallback(const mapping_msgs::AttachedCollisionObjectConstPtr &attached_object);

      void attachObject(const mapping_msgs::CollisionObject &obj);

      bool planToPosition(motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res);

      bool plan(std::vector<trajectory_msgs::JointTrajectoryPoint> &arm_path);

      bool isGoalConstraintSatisfied(const std::vector<double> &angles, const motion_planning_msgs::Constraints &goal);

      bool computeIK(const geometry_msgs::Pose &pose_stamped_msg, std::vector<double> jnt_pos, std::vector<double> &solution);

      bool computeFK(const std::vector<double> &jnt_pos, geometry_msgs::Pose &pose);

      void computeFKwithKDL(const std::vector<double> &jnt_pos, geometry_msgs::Pose &pose, int joint_num);

      bool initChain(std::string robot_description);

      void visualizeGoalPosition(const motion_planning_msgs::Constraints &goal);

      /* \brief Visualize the possible elbow poses when end-eff is at goal */
      void visualizeElbowPoses();

      void displayShortestPath();

      void printPath(FILE* fOut, const std::vector<std::vector<double> > path);

      void printPath(const std::vector<trajectory_msgs::JointTrajectoryPoint> &arm_path);

      void setArmToMapTransform(std::string &map_frame);

      void displayARAStarStates();

      /* \brief Visualize the collision model of the attached object */
      void visualizeAttachedObject(trajectory_msgs::JointTrajectory &traj_msg, int throttle);

      /* \brief Visualize the collision model of the attached object */
      void visualizeAttachedObject(const std::vector<double> &angles);

      void visualizeCollisionObjects();
  };
}

#endif

---

sbpl_arm_planner_node
Author(s): Benjamin Cohen/bcohen@seas.upenn.edu
autogenerated on Wed Feb 29 11:05:57 2012