コード例 #1
0
ファイル: drive2object.cpp プロジェクト: robotican/robotican
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;
    }




}
コード例 #2
0
ファイル: sensors.c プロジェクト: remina/AHRS_AGV
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);	
	}
}