Go to the documentation of this file.00001
00002
00003 #include <std_srvs/SetBool.h>
00004 #include <ros/ros.h>
00005 #include <std_msgs/String.h>
00006 #include <move_base_msgs/MoveBaseAction.h>
00007 #include <geometry_msgs/PointStamped.h>
00008 #include <std_srvs/Trigger.h>
00009 #include <tf/transform_listener.h>
00010 #include <actionlib/client/simple_action_client.h>
00011 #include <moveit/planning_scene_interface/planning_scene_interface.h>
00012
00013 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
00014
00015 moveit::planning_interface::PlanningSceneInterface *planning_scene_interface_ptr;
00016 tf::TransformListener *listener_ptr;
00017
00018 move_base_msgs::MoveBaseGoal get_pre_pick_pose();
00019 bool base_cmd(move_base_msgs::MoveBaseGoal goal);
00020
00021 MoveBaseClient *moveBaseClient_ptr;
00022
00023 bool moving=false;
00024 std::string object_name;
00025 double base_distance_from_object=0.55;
00026
00027
00028
00029 move_base_msgs::MoveBaseGoal get_pre_pick_pose(geometry_msgs::Point point) {
00030 tf::Transform dest_transform;
00031 move_base_msgs::MoveBaseGoal goal;
00032 try{
00033
00034 tf::StampedTransform transform_base;
00035 try {
00036 listener_ptr->lookupTransform("map", "base_link", ros::Time(0), transform_base);
00037
00038 }
00039 catch (tf::TransformException ex){
00040 ROS_ERROR("%s",ex.what());
00041 return goal;
00042 }
00043 tf::Vector3 v_obj(point.x,point.y,point.z);
00044
00045 tf::Vector3 v_base =transform_base.getOrigin();
00046
00047 tf::Vector3 v=v_obj-v_base;
00048 double yaw=atan2(v.y(),v.x());
00049 double away=base_distance_from_object/sqrt(v.x()*v.x()+v.y()*v.y());
00050 tf::Vector3 dest=v_base+v*(1-away);
00051
00052 dest_transform.setOrigin( dest );
00053 dest.setZ(0);
00054 tf::Quaternion q;
00055 q.setRPY(0.0, 0, yaw);
00056 dest_transform.setRotation(q);
00057
00058
00059
00060 }
00061
00062 catch (tf::TransformException ex){
00063 ROS_ERROR("%s",ex.what());
00064 return goal;
00065 }
00066
00067 goal.target_pose.header.frame_id = "map";
00068 goal.target_pose.header.stamp = ros::Time::now();
00069 goal.target_pose.pose.position.x=dest_transform.getOrigin().x();
00070 goal.target_pose.pose.position.y=dest_transform.getOrigin().y();
00071 goal.target_pose.pose.position.z=0;
00072 goal.target_pose.pose.orientation.x=dest_transform.getRotation().x();
00073 goal.target_pose.pose.orientation.y=dest_transform.getRotation().y();
00074 goal.target_pose.pose.orientation.z=dest_transform.getRotation().z();
00075 goal.target_pose.pose.orientation.w=dest_transform.getRotation().w();
00076 return goal;
00077 }
00078
00079
00080
00081 bool drive_go_cb(std_srvs::Trigger::Request &req,
00082 std_srvs::Trigger::Response &res)
00083 {
00084
00085
00086 ros::NodeHandle n;
00087 ros::ServiceClient uc_client = n.serviceClient<std_srvs::SetBool>("update_collision_objects");
00088 std_srvs::SetBool srv;
00089
00090 std::vector<std::string> ids;
00091 ids.push_back(object_name);
00092 std::map<std::string, moveit_msgs::CollisionObject> poses=planning_scene_interface_ptr->getObjects(ids);
00093 std::map<std::string, moveit_msgs::CollisionObject>::iterator it;
00094
00095 it = poses.find(object_name);
00096
00097 if (it != poses.end()) {
00098 moveit_msgs::CollisionObject obj=it->second;
00099
00100 if (!moving) moving=true;
00101
00102 move_base_msgs::MoveBaseGoal goal=get_pre_pick_pose(obj.primitive_poses[0].position);
00103
00104
00105
00106 srv.request.data=false;
00107 uc_client.call(srv);
00108
00109 if (base_cmd(goal)) {
00110 ROS_INFO("Reached position");
00111 res.message="Reached pre-picking position";
00112 res.success=true;
00113 }
00114 else{
00115 ROS_INFO("move_base failed");
00116 res.message="move_base failed";
00117 res.success=false;
00118 }
00119 moving=false;
00120 srv.request.data=true;
00121 uc_client.call(srv);
00122 return true;
00123 }
00124 else {
00125 ROS_ERROR("No object found");
00126 res.message="No object found";
00127 res.success=false;
00128 srv.request.data=true;
00129 uc_client.call(srv);
00130 return true;
00131 }
00132
00133
00134
00135
00136 }
00137
00138
00139
00140 bool base_cmd(move_base_msgs::MoveBaseGoal goal) {
00141
00142 ROS_INFO("[%s]: Sending goal", ros::this_node::getName().c_str());
00143
00144 moveBaseClient_ptr->sendGoal(goal);
00145 moveBaseClient_ptr->waitForResult();
00146
00147 if(moveBaseClient_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
00148 return true;
00149 }
00150 else {
00151 ROS_ERROR("[%s]: Navigation failed ", ros::this_node::getName().c_str());
00152 return false;
00153 }
00154 }
00155 int main(int argc, char **argv) {
00156
00157 ros::init(argc, argv, "drive2object_node");
00158 ros::NodeHandle n;
00159 ros::NodeHandle pn("~");
00160
00161
00162 pn.param<double>("base_distance_from_object", base_distance_from_object, 0.55);
00163 pn.param<std::string>("object_name", object_name, "can");
00164
00165 MoveBaseClient moveBaseClient("move_base", true);
00166
00167 while(!moveBaseClient.waitForServer(ros::Duration(5.0))){
00168 ROS_INFO("Waiting for the move_base action server to come up");
00169 }
00170
00171 moveBaseClient_ptr=&moveBaseClient;
00172
00173
00174 ros::ServiceServer service = n.advertiseService("drive2object_go", drive_go_cb);
00175
00176 tf::TransformListener listener;
00177 listener_ptr=&listener;
00178
00179 moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
00180 planning_scene_interface_ptr=&planning_scene_interface;
00181
00182 ros::ServiceClient uc_client = n.serviceClient<std_srvs::SetBool>("update_collision_objects");
00183 ROS_INFO("Waiting for update_collision service...");
00184 uc_client.waitForExistence();
00185 std_srvs::SetBool srv;
00186 srv.request.data=true;
00187 uc_client.call(srv);
00188
00189 ros::spin();
00190
00191 return 0;
00192 }
00193