diff --git a/src/ccf/controller/MoveItRobotArmController.cpp b/src/ccf/controller/MoveItRobotArmController.cpp index 64530a67792af5519156daf09fccf517aa29437a..1ba6e3af95701a0051d862b20da095bca8786db2 100644 --- a/src/ccf/controller/MoveItRobotArmController.cpp +++ b/src/ccf/controller/MoveItRobotArmController.cpp @@ -32,7 +32,13 @@ void MoveItRobotArmController::updateScene(ccf::SceneUpdateRequest &req) { } else { ccf::BinCheck srv; srv.request.id = req.ids[i]; - bin_check_client.call(srv); + + if (!bin_check_client.call(srv)) { + ROS_ERROR_STREAM("[MoveItRobotArmController] " + "Unable to call " << bin_check_client.getService() << + ". It " << (bin_check_client.exists() ? "exists" : "it does not exist") + << ". Assuming each object is a BOX."); + } if (srv.response.isBin) { object->set_type(Object_Type_BIN); @@ -98,7 +104,13 @@ bool MoveItRobotArmController::pickAndDrop(Object &robot, Object &object, Object ccf::PickDropService srv; srv.request.object = object.id(); srv.request.bin = bin.id(); - pick_drop_client.call(srv); + if (!pick_drop_client.call(srv)) { + ROS_ERROR_STREAM("[MoveItRobotArmController] " + "Unable to call " << pick_drop_client.getService() << + ". It " << (pick_drop_client.exists() ? "exists" : "it does not exist") + << "."); + return false; + } // we explicitly do not remove the object right now, so we do not call the super method return true; @@ -120,7 +132,13 @@ bool MoveItRobotArmController::pickAndPlace(Object &robot, Object &object, Objec srv.request.pose.position.x = location.pos().x(); srv.request.pose.position.y = location.pos().y(); srv.request.pose.position.z = location.pos().z(); - pick_place_client.call(srv); + if (!pick_place_client.call(srv)) { + ROS_ERROR_STREAM("[MoveItRobotArmController] " + "Unable to call " << pick_place_client.getService() << + ". It " << (pick_place_client.exists() ? "exists" : "it does not exist") + << "."); + return false; + } // we explicitly do not move the object right now, so we do not call the super method return true; @@ -129,10 +147,10 @@ bool MoveItRobotArmController::pickAndPlace(Object &robot, Object &object, Objec MoveItRobotArmController::MoveItRobotArmController(ros::NodeHandle &nodeHandle, const std::string &cellName) : RobotArmController(nodeHandle, cellName), - pick_drop_client(nodeHandle.serviceClient<ccf::PickDropService>("/pick_drop_service")), - pick_place_client(nodeHandle.serviceClient<ccf::PickPlaceService>("/pick_place_service")) { + pick_drop_client(nodeHandle.serviceClient<ccf::PickDropService>("/scene_controller/pick_drop_service")), + pick_place_client(nodeHandle.serviceClient<ccf::PickPlaceService>("/scene_controller/pick_place_service")) { - bin_check_client = nodeHandle.serviceClient<ccf::BinCheck>("/check_bin_service"); + bin_check_client = nodeHandle.serviceClient<ccf::BinCheck>("/scene_controller/check_bin_service"); get_scene_service = nodeHandle.advertiseService<ccf::SceneUpdateRequest, ccf::SceneUpdateResponse>( "updateCgvScene", [this](auto &req, auto &res) { diff --git a/src/ccf/util/scene_controller_node.cpp b/src/ccf/util/scene_controller_node.cpp index dd7e9e04bb16abf1bf20f74baa776256fbbe4c30..45bdbad7ce253d777c7aa37de101537ce2f6b0a5 100644 --- a/src/ccf/util/scene_controller_node.cpp +++ b/src/ccf/util/scene_controller_node.cpp @@ -804,7 +804,7 @@ int main(int argc, char **argv) { boost::bind(&manualScenePush, _1, boost::ref(n))); - ros::ServiceServer service = n.advertiseService("/check_bin_service", checkBin); + ros::ServiceServer service = n.advertiseService("check_bin_service", checkBin); ros::ServiceServer pick_drop_service = n.advertiseService<ccf::PickDropService::Request, ccf::PickDropService::Response>( "pick_drop_service",