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

add dependent controllers that get the forwarded selection of a main controller

parent 020e4ab0
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 81 additions and 35 deletions
config/config.yaml
+ 0
− 2
View file @ 064b11e3
connector_node_ros_cgv:
cgv_address: "tcp://*:6576"
max_grasp_approach_velocity: 0.2
max_grasp_approach_acceleration: 0.2
max_grasp_transition_velocity: 0.2
......
launch/cleaning/cleaning_double__dummy_controller_and_dummy_cgv.launch 0 → 100644
+ 11
− 0
View file @ 064b11e3
<launch>
<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
<node pkg="ccf" type="dummy_cgv_controller" name="my_first_dummy_cgv_controller" output="screen">
<param name="connection_address" type="string" value="tcp://*:6576" />
<param name="client_controllers" type="yaml" value="['tcp://127.0.0.1:6586']" />
</node>
<node pkg="ccf" type="dummy_cgv_controller" name="my_second_dummy_cgv_controller" output="screen">
<param name="connection_address" type="string" value="tcp://*:6586" />
<param name="client_controllers" type="yaml" value="[]" />
</node>
</launch>
launch/cleaning/cleaning_dummy_controller.launch
+ 9
− 1
View file @ 064b11e3
<launch>
<node pkg="ccf" type="dummy_cgv_controller" name="my_dummy_cgv_controller" output="screen" />
<arg name="connection_address" default="tcp://*:6576" />
<arg name="client_controllers" default="[]" />
<node pkg="ccf" type="dummy_cgv_controller" name="my_dummy_cgv_controller" output="screen">
<param name="connection_address" type="string" value="$(arg connection_address)" />
<param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
</node>
</launch>
launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch
+ 1
− 1
View file @ 064b11e3
<launch>
<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
<node pkg="ccf" type="dummy_cgv_controller" name="my_dummy_cgv_controller" output="screen" />
<include file="$(find ccf)/launch/cleaning/cleaning_dummy_controller.launch" />
</launch>
launch/cleaning/cleaning_robot_controller.launch
+ 6
− 1
View file @ 064b11e3
......@@ -2,12 +2,17 @@
<arg name="robot_ip" default="172.31.1.13" />
<arg name="load_gripper" default="true" />
<arg name="connection_address" default="tcp://*:6576" />
<arg name="client_controllers" default="[]" />
<include file="$(find ccf)/launch/cleaning/_robot_setup.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="load_gripper" value="$(arg load_gripper)"/>
</include>
<node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" />
<node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" >
<param name="connection_address" type="string" value="$(arg connection_address)" />
<param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
</node>
</launch>
\ No newline at end of file
launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch
+ 2
− 12
View file @ 064b11e3
<launch>
<arg name="robot_ip" default="172.31.1.13" />
<arg name="load_gripper" default="true" />
<include file="$(find ccf)/launch/cleaning/_robot_setup.launch" >
<arg name="robot_ip" value="$(arg robot_ip)" />
<arg name="load_gripper" value="$(arg load_gripper)"/>
</include>
<node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" />
<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
</launch>
\ No newline at end of file
<include file="$(find ccf)/launch/cleaning/cleaning_robot_controller.launch" />
</launch>
launch/cleaning/cleaning_simulation.launch
+ 7
− 1
View file @ 064b11e3
<launch>
<arg name="connection_address" default="tcp://*:6576" />
<arg name="client_controllers" default="[]" />
<include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/>
<node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" />
<node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" >
<param name="connection_address" type="string" value="$(arg connection_address)" />
<param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
</node>
</launch>
\ No newline at end of file
launch/cleaning/cleaning_simulation_and_dummy_cgv.launch
+ 3
− 7
View file @ 064b11e3
<launch>
<include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/>
<node pkg="ccf" type="dummy_cgv" name="dummy_cgv_instance" output="screen" />
<node pkg="ccf" type="moveit_cgv_controller" name="moveit_cgv_controller_instance" output="screen" />
</launch>
\ No newline at end of file
<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />
<include file="$(find ccf)/launch/cleaning/cleaning_simulation.launch" />
</launch>
src/ccf/controller/RobotArmController.cpp
+ 0
− 2
View file @ 064b11e3
......@@ -44,8 +44,6 @@ Object *RobotArmController::resolveObject(const std::string &id) {
void RobotArmController::sendScene() {
if (scene) { // meaning if the (smart) pointer is not a nullptr
scene->SerializeAsString();
ROS_INFO_STREAM("[ros2cgv] Sending scene with " << scene->objects().size() << " objects.");
sendToAll(sendSceneTopic, scene->SerializeAsString());
sceneUpdateAction();
......
src/dummy_cgv_controller.cpp
+ 19
− 3
View file @ 064b11e3
......@@ -19,19 +19,21 @@
#include "ccf/connection/NngConnection.h"
#include "ccf/util/NodeUtil.h"
const std::string NODE_NAME = "ros2cgv_dummy";
std::string NODE_NAME;
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
int main(int argc, char **argv) {
GOOGLE_PROTOBUF_VERIFY_VERSION;
NODE_NAME = ros::this_node::getName();
ros::init(argc, argv, NODE_NAME);
ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is
std::string cgv_address = getParameter<std::string>(n, "connection_address", "tcp://*:6576");
auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
DummyRobotArmController connector{n, NODE_NAME};
......@@ -41,6 +43,16 @@ int main(int argc, char **argv) {
connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
connector.addConnection(std::move(connection));
auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers.");
for (const auto &client : clientControllers) {
ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << ".");
std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
connector.addConnection(std::move(client_connection));
}
connector.loadScene(ros::package::getPath("ccf") + "/config/config_scene.yaml");
Object *robot = nullptr;
......@@ -53,9 +65,13 @@ int main(int argc, char **argv) {
ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](
auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n](
const Selection &selection) {
// forward the selection to the clients
connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"),
selection.SerializeAsString());
if (currentlyPickedBox.has_value()) {
ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
return;
......
src/dummy_rag_controller_a.cpp
+ 2
− 1
View file @ 064b11e3
......@@ -28,6 +28,7 @@ const std::string CELL_NAME = "place-a";
const std::string NODE_NAME = "rag_connector_dummy_a";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
int main(int argc, char **argv) {
......@@ -37,7 +38,7 @@ int main(int argc, char **argv) {
ros::NodeHandle n("ceti_connector"); // namespace
std::string cgv_address = getParameter<std::string>(n, "cgv_address", "tcp://*:6576");
auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
DummyRobotArmController connector{n, CELL_NAME};
......
src/dummy_rag_controller_b.cpp
+ 2
− 1
View file @ 064b11e3
......@@ -27,6 +27,7 @@ const std::string CELL_NAME = "place-b";
const std::string NODE_NAME = "rag_connector_dummy_b";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
int main(int argc, char **argv) {
......@@ -36,7 +37,7 @@ int main(int argc, char **argv) {
ros::NodeHandle n("ceti_connector"); // namespace
std::string cgv_address = getParameter<std::string>(n, "cgv_address", "tcp://*:6576");
auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
std::string cell_prefix = getParameter(n, "place_b_prefix", CELL_NAME);
......
src/moveit_cgv_controller.cpp
+ 19
− 3
View file @ 064b11e3
......@@ -18,19 +18,21 @@
#include "ccf/connection/NngConnection.h"
#include "ccf/util/NodeUtil.h"
const std::string NODE_NAME = "ros2cgv_moveit";
std::string NODE_NAME;
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
int main(int argc, char **argv) {
GOOGLE_PROTOBUF_VERIFY_VERSION;
NODE_NAME = ros::this_node::getName();
ros::init(argc, argv, NODE_NAME);
ros::NodeHandle n("connector_node_ros_cgv"); // namespace where the connection_address is
std::string cgv_address = getParameter<std::string>(n, "connection_address", "tcp://*:6576");
auto cgv_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
MoveItRobotArmController connector{n, NODE_NAME};
......@@ -40,6 +42,16 @@ int main(int argc, char **argv) {
connection->setSendTopic(getParameter(n, "topics/scene", NODE_NAME + "/scene/update"));
connector.addConnection(std::move(connection));
auto clientControllers = getPrivateParameter<std::vector<std::string>>("client_controllers");
ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to " << clientControllers.size() << " client controllers.");
for (const auto &client : clientControllers) {
ROS_INFO_STREAM("[" << NODE_NAME << "] Connecting to client at " << client << ".");
std::unique_ptr<NngConnection> client_connection = std::make_unique<NngConnection>(client, false);
client_connection->setSendTopic(getParameter<std::string>(n, "topics/selection", "selection"));
connector.addConnection(std::move(client_connection));
}
// scene loading is not required, the scene is updated by moveit
Object *robot = nullptr;
......@@ -52,9 +64,13 @@ int main(int argc, char **argv) {
ros::Timer timer = n.createTimer(ros::Duration(10), [&connector](
const ros::TimerEvent &event) { connector.sendScene(); }); // send a scene every ten seconds
auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin](
auto selectionMessageCallback = [&currentlyPickedBox, &connector, &robot, &selectedBox, &selectedBin, &n](
const Selection &selection) {
// forward the selection to the clients
connector.sendToAll(getParameter<std::string>(n, "topics/selection", "selection"),
selection.SerializeAsString());
if (currentlyPickedBox.has_value()) {
ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to accept selections while picking is in progress.");
return;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment