Skip to content
Snippets Groups Projects
Commit 7afc0342 authored by Johannes Mey's avatar Johannes Mey
Browse files

make topic configurable in place b

parent dad661d5
Branches
No related tags found
No related merge requests found
<launch>
<node pkg="ccf" type="dummy_rag_connector_b" name="dummy_rag_connector_b_1" output="screen">
<rosparam param="/ccf/place_b_prefix">place-b-1</rosparam>
</node>
</launch>
<launch>
<node pkg="ccf" type="dummy_rag_connector_b" name="dummy_rag_connector_b_2" output="screen">
<rosparam param="/ccf/place_b_prefix">place-b-2</rosparam>
</node>
</launch>
......@@ -38,7 +38,10 @@ int main(int argc, char **argv) {
std::string cgv_address = getParameter(n, "cgv_address", "tcp://*:6576");
std::string cell_prefix = getParameter(n, "place_b_prefix", CELL_NAME);
DummyRobotArmController connector{n, CELL_NAME};
connector.setSendSceneTopic(cell_prefix + "/scene");
// only for debugging (uncomment when running without the RAG part)
// connector.loadScene("/home/jm/work/git/ceti/models2021/src/ccf/config/config_pick_place_scene.json");
......@@ -46,8 +49,8 @@ int main(int argc, char **argv) {
// add an MQTT connection
std::string mqtt_address = getParameter(n, "mqtt/mqtt_address", "tcp://localhost:1883");
std::unique_ptr<MqttConnection> mqtt_connection = std::make_unique<MqttConnection>(mqtt_address, NODE_NAME);
mqtt_connection->addTopic(CELL_NAME + "/scene/init");
mqtt_connection->addTopic(CELL_NAME + "/command");
mqtt_connection->addTopic(connector.getInitSceneTopic());
mqtt_connection->addTopic(connector.getCommandTopic());
connector.addConnection(std::move(mqtt_connection));
ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000,
......@@ -104,7 +107,7 @@ int main(int argc, char **argv) {
};
connector.reactToPickAndPlaceMessage(pickPlaceLambda);
std::string reachabilityTopicPrefix = CELL_NAME + "/reachability/";
std::string reachabilityTopicPrefix = cell_prefix + "/reachability/";
auto sceneUpdateLambda = [&reachabilityTopicPrefix, &connector]() {
ROS_INFO_STREAM("[" << NODE_NAME << "] Scene was updated, sending new reachability information.");
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment