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