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); }
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); }
//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; }