00001 00003 // Move the base until a desired end-effector pose can be reached. 00004 // Andy Zelenak, 2017 00006 00007 #include <actionlib/client/simple_action_client.h> 00008 #include <move_base_msgs/MoveBaseAction.h> 00009 #include "move_base_to_manip/desired_robot_pose.h" 00010 #include "moveit/move_group_interface/move_group.h" 00011 #include "moveit_msgs/DisplayTrajectory.h" 00012 #include "std_srvs/Empty.h" 00013 00014 namespace move_base_to_manip 00015 { 00016 const double cartesian_motion(const std::vector<geometry_msgs::Pose>& waypoints, moveit_msgs::RobotTrajectory& trajectory, moveit::planning_interface::MoveGroup& moveGroup, ros::NodeHandle& nh); 00017 00018 void set_node_params(ros::NodeHandle &nh); 00019 00020 void setup_move_group(ros::NodeHandle& nh, moveit::planning_interface::MoveGroup& moveGroup); 00021 00022 void setup_base_marker(visualization_msgs::Marker& baseMarker, move_base_msgs::MoveBaseGoal& goal); 00023 00024 ros::ServiceClient clear_octomap_client; 00025 ros::ServiceClient clear_costmaps_client; 00026 00027 // An empty service call 00028 std_srvs::Empty empty_srv; 00029 } 00030 00031 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; 00032 00033 #define SUCCESS false 00034 #define FAILURE true