bool drive_go_cb(std_srvs::Trigger::Request &req,
std_srvs::Trigger::Response &res)
{
ros::NodeHandle n;
ros::ServiceClient uc_client = n.serviceClient<std_srvs::SetBool>("update_collision_objects");
std_srvs::SetBool srv;
std::vector<std::string> ids;
ids.push_back(object_name);
std::map<std::string, moveit_msgs::CollisionObject> poses=planning_scene_interface_ptr->getObjects(ids);
std::map<std::string, moveit_msgs::CollisionObject>::iterator it;
it = poses.find(object_name);
if (it != poses.end()) {
moveit_msgs::CollisionObject obj=it->second;
if (!moving) moving=true;
move_base_msgs::MoveBaseGoal goal=get_pre_pick_pose(obj.primitive_poses[0].position);
srv.request.data=false;
uc_client.call(srv);
if (base_cmd(goal)) {
ROS_INFO("Reached position");
res.message="Reached pre-picking position";
res.success=true;
}
else{
ROS_INFO("move_base failed");
res.message="move_base failed";
res.success=false;
}
moving=false;
srv.request.data=true;
uc_client.call(srv);
return true;
}
else {
ROS_ERROR("No object found");
res.message="No object found";
res.success=false;
srv.request.data=true;
uc_client.call(srv);
return true;
}
}
void arbitrator()
{
u8 i = 0;
if (arbitrate) {
for (i = 0; i < amp_size; i++)
{
if (p_amp[i]->flag) break;
}
this_layer = amp[i];
base_cmd(this_layer);
}
}