diff --git a/include/ccf/controller/RobotArmController.h b/include/ccf/controller/RobotArmController.h index 606955e71ae3379b60f066a59788dc66057e8dbd..f3d047eed5ce0312408a7f8ef4f9a18a67d6441c 100644 --- a/include/ccf/controller/RobotArmController.h +++ b/include/ccf/controller/RobotArmController.h @@ -33,6 +33,22 @@ private: std::string initSceneTopic; std::string sendSceneTopic; std::string commandTopic; +public: + const std::string &getSelectionTopic() const; + + void setSelectionTopic(const std::string &selectionTopic); + + const std::string &getInitSceneTopic() const; + + void setInitSceneTopic(const std::string &initSceneTopic); + + const std::string &getSendSceneTopic() const; + + void setSendSceneTopic(const std::string &sendSceneTopic); + + const std::string &getCommandTopic() const; + + void setCommandTopic(const std::string &commandTopic); protected: ros::NodeHandle nodeHandle; diff --git a/src/ccf/controller/RobotArmController.cpp b/src/ccf/controller/RobotArmController.cpp index bf1f0f9a1ba29cb9e78ebdcd018b953eacd0fdaf..e3bad545e4d7825110bf428d001c0ff7c537a760 100644 --- a/src/ccf/controller/RobotArmController.cpp +++ b/src/ccf/controller/RobotArmController.cpp @@ -8,8 +8,11 @@ #include <google/protobuf/text_format.h> #include <google/protobuf/util/json_util.h> +#include "ccf/util/NodeUtil.h" #include "ccf/controller/RobotArmController.h" +using CetiRosToolbox::getParameter; + void RobotArmController::receive(const std::string &channel, const std::string &data) { ROS_WARN_STREAM("[ros2cgv] Received message on channel " << channel << "."); if (channel == selectionTopic) { @@ -90,21 +93,11 @@ RobotArmController::RobotArmController(const ros::NodeHandle &nodeHandle, const selectionAction([](const Selection &s) {}), cellName(cellName) { - selectionTopic = "selection"; - initSceneTopic = cellName + "/scene/init"; - sendSceneTopic = cellName + "/scene/update"; - commandTopic = cellName + "/command"; + selectionTopic = getParameter(nodeHandle, "topics/selection","selection"); + initSceneTopic = getParameter(nodeHandle, "topics/initScene",cellName + "/scene/init"); + sendSceneTopic = getParameter(nodeHandle, "topics/sendScene",cellName + "/scene/update"); + commandTopic = getParameter(nodeHandle, "topics/command",cellName + "/command"); - if (!nodeHandle.getParam("topics/selection", selectionTopic)) { - ROS_WARN_STREAM("[ros2cgv] Could not get string value for " << nodeHandle.getNamespace() - << "/topics/selection from param server, using default " - << selectionTopic); - } - if (!nodeHandle.getParam("topics/scene", initSceneTopic)) { - ROS_WARN_STREAM("[ros2cgv] Could not get string value for " << nodeHandle.getNamespace() - << "/topics/scene from param server, using default " - << initSceneTopic); - } } bool RobotArmController::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { @@ -169,3 +162,35 @@ void RobotArmController::loadScene(const std::string &sceneFile) { ROS_WARN_STREAM("[ros2cgv-dummy] Scene invalid! partial content: " << newScene.ShortDebugString()); } } + +const std::string &RobotArmController::getSelectionTopic() const { + return selectionTopic; +} + +void RobotArmController::setSelectionTopic(const std::string &selectionTopic) { + RobotArmController::selectionTopic = selectionTopic; +} + +const std::string &RobotArmController::getInitSceneTopic() const { + return initSceneTopic; +} + +void RobotArmController::setInitSceneTopic(const std::string &initSceneTopic) { + RobotArmController::initSceneTopic = initSceneTopic; +} + +const std::string &RobotArmController::getSendSceneTopic() const { + return sendSceneTopic; +} + +void RobotArmController::setSendSceneTopic(const std::string &sendSceneTopic) { + RobotArmController::sendSceneTopic = sendSceneTopic; +} + +const std::string &RobotArmController::getCommandTopic() const { + return commandTopic; +} + +void RobotArmController::setCommandTopic(const std::string &commandTopic) { + RobotArmController::commandTopic = commandTopic; +}