From 7c90098c3592710121a8c52e22d006781fe3dc66 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Thu, 22 Jul 2021 02:27:48 +0200
Subject: [PATCH] Fix service namespace issues. add checks. picking works
again.
---
.../controller/MoveItRobotArmController.cpp | 30 +++++++++++++++----
src/ccf/util/scene_controller_node.cpp | 2 +-
2 files changed, 25 insertions(+), 7 deletions(-)
diff --git a/src/ccf/controller/MoveItRobotArmController.cpp b/src/ccf/controller/MoveItRobotArmController.cpp
index 64530a6..1ba6e3a 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 dd7e9e0..45bdbad 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",
--
GitLab