コード例 #1
0
	void color_detection(const sensor_msgs::PointCloud2ConstPtr& cloud){
	getObjLocsSrv.request.cloud= *cloud;
	ros::NodeHandle node;
	ros::ServiceClient objClient= node.serviceClient<nao_world_msgs::ObjectLocations>("ObjectLocations");
	ros::Publisher _markerPub = node.advertise<visualization_msgs::MarkerArray>("objects", 1000);
	
	visualization_msgs::MarkerArray ma;

	if(objClient.call(getObjLocsSrv)){
		visualization_msgs::MarkerArray currBoxLocs= getObjLocsSrv.response.boxLocs;
		visualization_msgs::MarkerArray currBallLocs= getObjLocsSrv.response.ballLocs;
		for(int i=0; i<currBoxLocs.markers.size(); i++){
	  		ma.markers.push_back(currBoxLocs.markers[i]);
		}

		for(int i=0; i<currBallLocs.markers.size(); i++){
			ma.markers.push_back(currBallLocs.markers[i]);
		}
		

		for(int i=0; i<getObjLocsSrv.response.boxes.size(); i++){
			std::stringstream currLoc;
			currLoc << "box" << i << " pos-" << getObjLocsSrv.response.boxes[i].x 
				<< "-" << getObjLocsSrv.response.boxes[i].y;
			currentState.setBooleanPredicate("at", currLoc.str(), true);
			currentState.setBooleanPredicate("clear", currLoc.str(), false);
			
		}

		for(int i=0; i<getObjLocsSrv.response.balls.size(); i++){
			std::stringstream currLoc;
			currLoc << "ball" << i << " pos-" << getObjLocsSrv.response.balls[i].x 
				<< "-" << getObjLocsSrv.response.balls[i].y;
			currentState.setBooleanPredicate("at", currLoc.str(), true);
			currentState.setBooleanPredicate("clear", currLoc.str(), false);
			
		}
		


	}

	_markerPub.publish(ma);	


   	}
コード例 #2
0
void StatusPublisher::finishedStateEstimation(bool success, const SymbolicState & state, const SymbolicState & goal)
{
    SymbolicState::OStreamMode::forceNewlines = true;
    stringstream ss;
    ss << "Goal:" << std::endl;
    goal.toPDDLGoal(ss);
    ss << state;
    SymbolicState::OStreamMode::forceNewlines = false;
    publishStatus(ContinualPlanningStatus::STATE_ESTIMATION,
            success ? int(ContinualPlanningStatus::SUCCESS) : int(ContinualPlanningStatus::FAILURE), ss.str());
}
コード例 #3
0
    void ActionExecutorPickupObject::updateState(const actionlib::SimpleClientGoalState & actionReturnState,
            const tidyup_msgs::GraspObjectResult & result,
            const DurativeAction & a, SymbolicState & current)
    {
        ROS_INFO("PickupObject returned result");
        ROS_ASSERT(a.parameters.size() == 4);
        string location = a.parameters[0];
        string object = a.parameters[1];
        string static_object = a.parameters[2];
        string arm = a.parameters[3];
// TODO: set grasp in symbolic state?       result.grasp;
        if(actionReturnState == actionlib::SimpleClientGoalState::SUCCEEDED) {
            ROS_INFO("PickupObject succeeded.");
            current.setBooleanPredicate("grasped", object + " " + arm, true);
            current.setBooleanPredicate("on", object + " " + static_object, false);
            current.setBooleanPredicate("graspable-from", object + " " + location + " left_arm", false);
            current.setBooleanPredicate("graspable-from", object + " " + location + " right_arm", false);
            //current.setObjectFluent("object-detected-from", object, "robot_location"); // FIXME hack, unset this
            // TODO: set false for other locations as well?
        }
        current.setObjectFluent("arm-state", arm, "arm_unknown");
        current.setBooleanPredicate("searched", location, false);
        current.setBooleanPredicate("recent-detected-objects", location, false);
    }
コード例 #4
0
	//sets the current location for the robot and the observed objects
        bool StateCreatorRobotPose::fillState(SymbolicState & state)
        {
		//call service to set robot location
		ros::NodeHandle node;
		ros::ServiceClient robotClient= node.serviceClient<nao_world_msgs::RobotLocation>("RobotLocation");
		nao_world_msgs::RobotLocation getRobotLocsSrv;
		if(robotClient.call(getRobotLocsSrv)){
			std::stringstream currLoc, currLoc1;
			currLoc << "robot " << "pos-" << getRobotLocsSrv.response.robot_grid_cell.y 
				<< "-" << getRobotLocsSrv.response.robot_grid_cell.x;
		  currLoc1 << "pos-" << getRobotLocsSrv.response.robot_grid_cell.y 
				<< "-" << getRobotLocsSrv.response.robot_grid_cell.x;
			state.setBooleanPredicate("at", currLoc.str(), true);
			state.setBooleanPredicate("clear", currLoc1.str(), false);
			
			ROS_INFO("robot location now %d, %d", getRobotLocsSrv.response.robot_grid_cell.x, getRobotLocsSrv.response.robot_grid_cell.y);
			
			
			geometry_msgs::Quaternion robotOrientation= getRobotLocsSrv.response.robotLocation.pose.orientation;
			 // the incoming geometry_msgs::Quaternion is transformed to a tf::Quaterion
			tf::Quaternion quat;
			tf::quaternionMsgToTF(robotOrientation, quat);

			// the tf::Quaternion has a method to acess roll pitch and yaw
			double roll, pitch, yaw;
			tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
			
			string orientation= "robot dir-";
			if(yaw<=0.785 && yaw>-0.785){//(yaw<=M_PI/4 && yaw>-1*M_PI/4){
				orientation+="west";
			}
			if(yaw<=-0.785 && yaw>-2.35){//(yaw<=-1*M_PI/4 && yaw>-3*M_PI/4){
				orientation+= "north";
			}
			if(yaw<=-2.35 || yaw>2.35){//((yaw<=-3/4*M_PI && yaw>= -1*M_PI) || (yaw>(3/4*M_PI) && yaw <= M_PI)){
				orientation+= "east";
			}
			if(yaw>=0.785 && yaw<2.35){//(yaw>=(M_PI/4) && yaw<(3/4*M_PI)){
				orientation+= "south";
			}
			
			ROS_INFO("%f", yaw);

			state.setBooleanPredicate("Orientation", orientation, true);

			ROS_INFO("robot location updated successfully");
		}
		else{	
			ROS_INFO("failed to call service RobotLocation");
		}

		currentState= state;

		//call service to get object locations
		ros::Subscriber cameraImg= node.subscribe("/xtion/depth_registered/points", 1, color_detection);
		


		if(s_PublishLocationsAsMarkers)
               		publishLocationsAsMarkers(state);
		return true;


        }
コード例 #5
0
namespace nao_actions
{

    nao_world_msgs::ObjectLocations getObjLocsSrv;
    SymbolicState currentState;

    StateCreatorRobotPose::StateCreatorRobotPose()
    {
        ros::NodeHandle nhPriv("~");
        ros::NodeHandle nh;
	nhPriv.getParam("cell_size", cell_size);
	nhPriv.getParam("grid_size", grid_size);
	nhPriv.getParam("robotLoc", robotLoc);
	nhPriv.getParam("robotDir", robotDir);
	nhPriv.getParam("goalLoc", goalLoc);
	nhPriv.getParam("boxLocs", xmlboxLocs);
	nhPriv.getParam("ballLocs", xmlballLocs);
	nhPriv.getParam("boxes", xmlboxes);
	nhPriv.getParam("balls", xmlballs);

	ROS_ASSERT(xmlboxLocs.getType() == XmlRpc::XmlRpcValue::TypeArray);
	ROS_ASSERT(xmlballLocs.getType() == XmlRpc::XmlRpcValue::TypeArray);
	ROS_ASSERT(xmlboxes.getType() == XmlRpc::XmlRpcValue::TypeArray);
	ROS_ASSERT(xmlballs.getType() == XmlRpc::XmlRpcValue::TypeArray);

	//copying xmlrpc to vector
	for(int i=0; i<xmlboxLocs.size(); i++){
		boxLocs.push_back(xmlboxLocs[i]);
		boxes.push_back(xmlboxes[i]);
	}
	for(int i=0; i<xmlballLocs.size(); i++){
		ballLocs.push_back(xmlballLocs[i]);
		balls.push_back(xmlballs[i]);
	}

        //nhPriv.param("nav_target_tolerance_xy", _goalToleranceXY, 0.5);
        //nhPriv.param("nav_target_tolerance_yaw", _goalToleranceYaw, 0.26);  //15deg

        //bool relative;
      /*  nhPriv.param("nav_target_tolerance_relative_to_move_base", relative, false);
        if(relative) {
            // relative mode: 1. get the namespace for base_local_planner
            std::string base_local_planner_ns;
            if(!nhPriv.getParam("nav_base_local_planner_ns", base_local_planner_ns)) {
                ROS_WARN("nav_target_tolerance_relative_to_move_base set, but nav_base_local_planner_ns not set - trying to estimate");
                std::string local_planner;
                if(!nh.getParam("move_base_node/base_local_planner", local_planner)
                        && !nh.getParam("move_base/base_local_planner", local_planner)) {
                    ROS_ERROR("move_base(_node)/base_local_planner not set - falling back to absolute mode.");
                } else {
                    // dwa_local_planner/DWAPlannerROS -> DWAPlannerROS
                    std::string::size_type x = local_planner.find_last_of("/");
                    if(x == std::string::npos)
                        base_local_planner_ns = local_planner;
                    else
                        base_local_planner_ns = local_planner.substr(x + 1);
                    ROS_INFO("Estimated base_local_planner_ns to %s.", base_local_planner_ns.c_str());
                }
            }
            
            if(!base_local_planner_ns.empty()) { // success: 2. get the xy_goal_tolerance
                double move_base_tol_xy;
                if(!nh.getParam(base_local_planner_ns + "/xy_goal_tolerance", move_base_tol_xy)) {
                    ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
                            << (base_local_planner_ns + "/xy_goal_tolerance") << " was not set"
                            << " - falling back to absolute mode");
                } else { // 2. add move_base's tolerance to our relative tolerance
                    _goalToleranceXY += move_base_tol_xy;
                }

                double move_base_tol_yaw;
                if(!nh.getParam(base_local_planner_ns + "/yaw_goal_tolerance", move_base_tol_yaw)) {
                    ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
                            << (base_local_planner_ns + "/yaw_goal_tolerance") << " was not set"
                            << " - falling back to absolute mode");
                } else { // 2. add move_base's tolerance to our relative tolerance
                    _goalToleranceYaw += move_base_tol_yaw;
                }
            }
        }

            ROS_INFO("Tolerance for accepting nav goals set to %f m, %f deg.",
                    _goalToleranceXY, angles::to_degrees(_goalToleranceYaw));
	*/
            if(s_PublishLocationsAsMarkers) {
                _markerPub = nh.advertise<visualization_msgs::MarkerArray>("robot_pose_markers", 5, true);
                ROS_INFO("marker topic: %s", _markerPub.getTopic().c_str());
            }
        }

        StateCreatorRobotPose::~StateCreatorRobotPose()
        {
        }

        void StateCreatorRobotPose::initialize()
        {
            

        }

	void color_detection(const sensor_msgs::PointCloud2ConstPtr& cloud){
	getObjLocsSrv.request.cloud= *cloud;
	ros::NodeHandle node;
	ros::ServiceClient objClient= node.serviceClient<nao_world_msgs::ObjectLocations>("ObjectLocations");
	ros::Publisher _markerPub = node.advertise<visualization_msgs::MarkerArray>("objects", 1000);
	
	visualization_msgs::MarkerArray ma;

	if(objClient.call(getObjLocsSrv)){
		visualization_msgs::MarkerArray currBoxLocs= getObjLocsSrv.response.boxLocs;
		visualization_msgs::MarkerArray currBallLocs= getObjLocsSrv.response.ballLocs;
		for(int i=0; i<currBoxLocs.markers.size(); i++){
	  		ma.markers.push_back(currBoxLocs.markers[i]);
		}

		for(int i=0; i<currBallLocs.markers.size(); i++){
			ma.markers.push_back(currBallLocs.markers[i]);
		}
		

		for(int i=0; i<getObjLocsSrv.response.boxes.size(); i++){
			std::stringstream currLoc;
			currLoc << "box" << i << " pos-" << getObjLocsSrv.response.boxes[i].x 
				<< "-" << getObjLocsSrv.response.boxes[i].y;
			currentState.setBooleanPredicate("at", currLoc.str(), true);
			currentState.setBooleanPredicate("clear", currLoc.str(), false);
			
		}

		for(int i=0; i<getObjLocsSrv.response.balls.size(); i++){
			std::stringstream currLoc;
			currLoc << "ball" << i << " pos-" << getObjLocsSrv.response.balls[i].x 
				<< "-" << getObjLocsSrv.response.balls[i].y;
			currentState.setBooleanPredicate("at", currLoc.str(), true);
			currentState.setBooleanPredicate("clear", currLoc.str(), false);
			
		}
		


	}

	_markerPub.publish(ma);	


   	}



	//sets the current location for the robot and the observed objects
        bool StateCreatorRobotPose::fillState(SymbolicState & state)
        {
		//call service to set robot location
		ros::NodeHandle node;
		ros::ServiceClient robotClient= node.serviceClient<nao_world_msgs::RobotLocation>("RobotLocation");
		nao_world_msgs::RobotLocation getRobotLocsSrv;
		if(robotClient.call(getRobotLocsSrv)){
			std::stringstream currLoc, currLoc1;
			currLoc << "robot " << "pos-" << getRobotLocsSrv.response.robot_grid_cell.y 
				<< "-" << getRobotLocsSrv.response.robot_grid_cell.x;
		  currLoc1 << "pos-" << getRobotLocsSrv.response.robot_grid_cell.y 
				<< "-" << getRobotLocsSrv.response.robot_grid_cell.x;
			state.setBooleanPredicate("at", currLoc.str(), true);
			state.setBooleanPredicate("clear", currLoc1.str(), false);
			
			ROS_INFO("robot location now %d, %d", getRobotLocsSrv.response.robot_grid_cell.x, getRobotLocsSrv.response.robot_grid_cell.y);
			
			
			geometry_msgs::Quaternion robotOrientation= getRobotLocsSrv.response.robotLocation.pose.orientation;
			 // the incoming geometry_msgs::Quaternion is transformed to a tf::Quaterion
			tf::Quaternion quat;
			tf::quaternionMsgToTF(robotOrientation, quat);

			// the tf::Quaternion has a method to acess roll pitch and yaw
			double roll, pitch, yaw;
			tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
			
			string orientation= "robot dir-";
			if(yaw<=0.785 && yaw>-0.785){//(yaw<=M_PI/4 && yaw>-1*M_PI/4){
				orientation+="west";
			}
			if(yaw<=-0.785 && yaw>-2.35){//(yaw<=-1*M_PI/4 && yaw>-3*M_PI/4){
				orientation+= "north";
			}
			if(yaw<=-2.35 || yaw>2.35){//((yaw<=-3/4*M_PI && yaw>= -1*M_PI) || (yaw>(3/4*M_PI) && yaw <= M_PI)){
				orientation+= "east";
			}
			if(yaw>=0.785 && yaw<2.35){//(yaw>=(M_PI/4) && yaw<(3/4*M_PI)){
				orientation+= "south";
			}
			
			ROS_INFO("%f", yaw);

			state.setBooleanPredicate("Orientation", orientation, true);

			ROS_INFO("robot location updated successfully");
		}
		else{	
			ROS_INFO("failed to call service RobotLocation");
		}

		currentState= state;

		//call service to get object locations
		ros::Subscriber cameraImg= node.subscribe("/xtion/depth_registered/points", 1, color_detection);
		


		if(s_PublishLocationsAsMarkers)
               		publishLocationsAsMarkers(state);
		return true;


        }

    
       
    /**
     * Publishes locations for boxes, balls and the robot
     */
    void StateCreatorRobotPose::publishLocationsAsMarkers(const SymbolicState & state)
    {
	/*ros::NodeHandle nhPriv("~");
	nhPriv.getParam("robotLoc", robotLoc);
	robotLoc= robotLoc.substr(4);
	int index= robotLoc.find("-");
	double robotLocX= atof((robotLoc.substr(0, index).c_str()))*cell_size + 0.5 * cell_size;
	double robotLocY= atof((robotLoc.substr(index+1).c_str()))*cell_size + 0.5 * cell_size;
	ros::NodeHandle node;
	visualization_msgs::MarkerArray ma;
	visualization_msgs::Marker robLoc;
	robLoc.ns= "robot";
	robLoc.id= 0;
	robLoc.pose.position.x= robotLocX;
	robLoc.pose.position.y= robotLocY;
	robLoc.header.frame_id= "/base_link";
	robLoc.header.stamp= ros::Time::now();
	robLoc.type= visualization_msgs::Marker::ARROW;
	robLoc.action= visualization_msgs::Marker::ADD;
    	robLoc.scale.x= 100;
    	robLoc.scale.y= 100;
    	robLoc.scale.z= 10;
    	robLoc.color.r= 1.0;
    	robLoc.color.a= 1.0;
	ma.markers.push_back(robLoc);
    
   

	nhPriv.getParam("boxes", xmlboxes);
	nhPriv.getParam("boxLocs", xmlboxLocs);
	ROS_ASSERT(xmlboxLocs.getType() == XmlRpc::XmlRpcValue::TypeArray);
	ROS_ASSERT(xmlboxes.getType() == XmlRpc::XmlRpcValue::TypeArray);
	visualization_msgs::Marker box;
	double boxX, boxY;
	std::string currBoxLoc;
	for(int i=0; i<xmlboxes.size(); i++){
		box.ns= static_cast<std::string>(xmlboxes[i]);
		box.id= i;
		currBoxLoc= static_cast<std::string>(xmlboxLocs[i]);
		currBoxLoc= currBoxLoc.substr(4);
		index= currBoxLoc.find("-");		
		boxX= atof((currBoxLoc.substr(0,index).c_str()))*cell_size + 0.5 * cell_size;
		boxY= atof((currBoxLoc.substr(index+1).c_str()))*cell_size + 0.5 * cell_size;
        
		double box_size = 0; //25;
		// Check if the robot is holding this box.
		Predicate bp;
		bp.name= "Holding";
		std::vector<string> parameters;
		parameters.push_back("robot");
		parameters.push_back(box.ns);
		bp.parameters= parameters;
		bool value = true;
		if (state.hasBooleanPredicate(bp, &value)) {
		    box.pose.position.x= robotLocX;
		    box.pose.position.y= robotLocY;
		    box.pose.position.z= box_size;
		    box.scale.x= 15;
		    box.scale.y= 15;
		    box.scale.z= 15;
		} else {
		    box.pose.position.x= boxX;
		    box.pose.position.y= boxY;
		    box.pose.position.z= box_size;
		    box.scale.x= 25;
		    box.scale.y= 25;
		    box.scale.z= 25;
		}
		
		box.header.frame_id= "/base_link";
		box.header.stamp= ros::Time::now();
		box.type= visualization_msgs::Marker::CUBE;
		box.action= visualization_msgs::Marker::ADD;
		box.color.g= 1.0;
		box.color.a= 1.0;
		ma.markers.push_back(box);
	}	

	nhPriv.getParam("balls", xmlballs);
	nhPriv.getParam("ballLocs", xmlballLocs);
	ROS_ASSERT(xmlballLocs.getType() == XmlRpc::XmlRpcValue::TypeArray);
	ROS_ASSERT(xmlballs.getType() == XmlRpc::XmlRpcValue::TypeArray);
	visualization_msgs::Marker ball;
	double ballX, ballY;
	std::string currballLoc;
	for(int i=0; i<xmlballs.size(); i++){
		ball.ns= static_cast<std::string>(xmlballs[i]);
		ball.id= 0;
		currballLoc= static_cast<std::string>(xmlballLocs[i]);
		currballLoc= currballLoc.substr(4);
		index= currballLoc.find("-");		
		ballX= atof((currballLoc.substr(0,index).c_str()))*cell_size + 0.5 * cell_size;
		ballY= atof((currballLoc.substr(index+1).c_str()))*cell_size + 0.5 * cell_size;
        
		ball.pose.position.x= ballX;
		ball.pose.position.y= ballY;
		ball.header.frame_id= "/base_link";
		ball.header.stamp= ros::Time::now();
		ball.type= visualization_msgs::Marker::SPHERE;
		ball.action= visualization_msgs::Marker::ADD;
		ball.scale.x= 25;
		ball.scale.y= 25;
		ball.scale.z= 25;
		ball.color.b= 1.0;
		ball.color.a= 1.0;
		ma.markers.push_back(ball);
	}
*/

	visualization_msgs::MarkerArray ma;
/*

	ros::NodeHandle node;
	ros::ServiceClient client = node.serviceClient<nao_world_msgs::RobotLocation>("RobotLocation");
	nao_world_msgs::RobotLocation srv;
	visualization_msgs::MarkerArray ma;
	if(client.call(srv)){
		visualization_msgs::Marker robLoc;
		robLoc.ns= "robot";
		robLoc.id= 0;
		robLoc.pose= srv.response.robotLocation.pose;
		robLoc.header= srv.response.robotLocation.header;
		robLoc.type= visualization_msgs::Marker::ARROW;
		robLoc.action= visualization_msgs::Marker::ADD;
		ma.markers.push_back(robLoc);
	}
	else{
		ROS_ERROR("Cannot extract current robot location");
		return;
	}
*/
	
	ros::NodeHandle node;
	ros::ServiceClient robotClient= node.serviceClient<nao_world_msgs::RobotLocation>("RobotLocation");
	nao_world_msgs::RobotLocation getRobotLocsSrv;
	visualization_msgs::Marker robotLocMarker;
	if(robotClient.call(getRobotLocsSrv)){
		robotLocMarker.header.frame_id= getRobotLocsSrv.response.robotLocation.header.frame_id;
	  	robotLocMarker.header.stamp= getRobotLocsSrv.response.robotLocation.header.stamp;
		robotLocMarker.ns= "Robot";
		robotLocMarker.id= 0;
		robotLocMarker.pose= getRobotLocsSrv.response.robotLocation.pose;
		robotLocMarker.type= visualization_msgs::Marker::ARROW;
		robotLocMarker.action= visualization_msgs::Marker::ADD;
		robotLocMarker.color.b= 1.0;
		robotLocMarker.color.a= 1.0;

		ma.markers.push_back(robotLocMarker);
		_markerPub.publish(ma);	
	}

	//call to service to get the current positions of the balls and boxes
	ros::ServiceClient objClient= node.serviceClient<nao_world_msgs::ObjectLocations>("ObjectLocations");
	//subscribe to the camera images
	ros::Subscriber cameraImg= node.subscribe("/xtion/depth_registered/points", 1, color_detection);


    }

};