diff --git a/README.md b/README.md index 9578719d8317abf6a037903360df2ab4085d0dc4..466a9436318595f6aee937776ce3f6565a0718a3 100644 --- a/README.md +++ b/README.md @@ -1,123 +1,3 @@ # CCF: The CeTI Cobotic Framework -**TODO** the documentation does not represent the current state of the framework and needs to be updated! - -This ROS package provides a connection between a ROS-based model and the [CGV framework](https://github.com/sgumhold/cgv). -The purpose of this connection is to use the CGV framework to select and move elements from a scene provided by ROS -using an AR controller. - -For the communication between the two components, messages encoded using -[Protocol Buffers](https://developers.google.com/protocol-buffers) (v3) are exchanged using -[NNG](https://nng.nanomsg.org/). - -Since the package is under active development, it contains several demonstrators in varying development states. -In particular, besides the main demonstrator it contains several tests of NNG communication patterns that can be used -for internal and external testing. - -## Generating the ROS-Documentation - -- Source the workspace - ``` - $ source devel/setup.bash - ``` -- Generate the documentation - ``` - $ rosdoc_lite `rospack find ccf` - ``` -- You can view the generated files in you browser by opening the generated `index.html` - ``` - xdg-open `rospack find ccf`/doc/html/index.html - ``` -## Running the Tests - -1. Build the package (this is not done automatically by the following commands) - ```bash - catkin build - ``` -2. Build the tests - ```bash - catkin build --make-args tests - ``` -3. Run the tests - ```bash - rostest -t ccf test_NodeUtil.test - ``` - -## CGV Dummy Controller Demonstrator - -This demonstrator contains two mock-up nodes that can serve as stand-ins for future functional implementations. - -- `dummy_cgv` is a mock-up of the cgv framework. it connects to the ROS `ccf` and waits for it to send a scene. - Once a `Scene` is received, it waits two seconds and then sends a selection of a random `Box` from the scene (if there - is one). After another two seconds, the `bin` is selected. -- `dummy_ccf` is a mock-up of the ros end of the connection, accepting connections from the CGV framework. - When receiving a message on topic `/ccf/send_scene`, it sends a scene to the framework. It furthermore is - able to receive object `Selection` messages (and currently simply prints a notification). - -### Running the Example -- Checkout the project using git. - Since the CMake build uses git submodules, do not remove the git information! - It is not required to checkout the submodule manually, this is done using CMake. -- Build the project. -- Start ROS - ``` - $ roscore - ``` -- Start the `dummy_cgv_controller` and `dummy_cgv` nodes in two terminals. - Both are in the `ccf` namespace. - ``` - $ rosrun ccf dummy_cgv_controller - ``` - Note that the dummy_cgv tries to connect to the dummy_cgv_controller, so as long as the latter is not running, - warnings are logged. - ``` - $ rosrun ccf dummy_cgv - ``` -- Trigger a transmission by publishing a string message to `/ccf/send_scene`. - ``` - $ rostopic pub -1 /ccf/send_scene std_msgs/String "Some Message" - ``` - The `dummy_cgv` node command will show the received scene. With a delay of two seconds, selections of a box and a bin - will be logged by the `dummy_cgv_controller` - -### Running the demo - -#### Configuration - -- configure the scene (*not* required when using the dummy controller): - ```bash - rostopic pub -1 /scene_config std_msgs/String "\"$(roscat ccf config_scene.yaml)\"" - ``` - -#### Execution - -- pick an object by id: `rostopic pub -1 /robot/pick std_msgs/String "objectRed1"` -- place a picked object into a bin by id: `rostopic pub -1 /robot/place std_msgs/String "binRed"` - -### Connect to the CGV framework - -- **CASE 1** The robot controller is *not* accepting connections, but the AR server is. - - ensure a SSH server is running on the AR server - - On the robot controller, connect to it - ``` - ssh stadmin@141.76.65.71 -R 6576:localhost:6576 - ``` - -### Using the Example with Docker - -The following instructions can be used to run either the cgv dummy or the ros-connector dummy from within a docker container. -All commands involving docker must be run as root or as a user with docker privileges. - -- Create a container called `ccf` - ``` - $ docker build --tag ccf . - ``` -- Run the container, exposing the port the receiver listens on, `6576`. - ``` - $ docker run -it -p 6576:6576 ros4ceti/cgv:latest roslaunch ccf dummy_cgv_controller.launch - ``` - Run the CGV dummy: - ``` - $ docker run -it --network host ros4ceti/cgv:latest roslaunch ccf dummy_cgv.launch - ``` -- The dummy_cgv_controller now is available and awaits NNG messages at `localhost:6576`. +**TODO** An updated documentation will follow soon. \ No newline at end of file diff --git a/config/config.yaml b/config/config.yaml index 0056a34c3fe2d9622925fc12a6a799a835657619..a81eb7da54f9b8dc65dc42d5db5c047369434c59 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,4 +1,4 @@ -connector_node_ros_cgv: +connector_node_ros_ccf: topics: selection: "selection" max_grasp_approach_velocity: 0.2 diff --git a/config/config_scene.yaml b/config/config_scene.yaml deleted file mode 100644 index 2620d0c0e5242d87d6dfbdc8d2c6e4b39d6e6c18..0000000000000000000000000000000000000000 --- a/config/config_scene.yaml +++ /dev/null @@ -1,20 +0,0 @@ -{ 'objects': [ - { 'id': 'tablePillar1','pos': { 'x': 0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } }, - { 'id': 'tablePillar2','pos': { 'x': -0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } }, - { 'id': 'tablePillar3','pos': { 'x': -0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } }, - { 'id': 'tablePillar4','pos': { 'x': 0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } }, - { 'id': 'table','pos': { 'z': 0.7 },'size': { 'length': 1.6,'width': 1.6,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } }, - { 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, - { 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } }, - { 'id': 'binGreen','type': 'BIN','pos': { 'x': 0.46,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'g': 1 } }, - { 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } }, - { 'id': 'objectRed2','type': 'BOX','pos': { 'x': 0.25,'y': -0.2,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'w': 1 },'color': { 'r': 1 } }, - { 'id': 'objectRed3','type': 'BOX','pos': { 'x': -0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } }, - { 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.45,'y': -0.3,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } }, - { 'id': 'objectGreen2','type': 'BOX','pos': { 'x': -0.45,'y': -0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } }, - { 'id': 'objectGreen3','type': 'BOX','pos': { 'x': 0.1,'y': -0.3,'z': 0.819 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } }, - { 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } }, - { 'id': 'objectBlue2','type': 'BOX','pos': { 'x': -0.3,'y': -0.3,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } }, - { 'id': 'objectBlue3','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } }, - { 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } -] } diff --git a/launch/cell-sync/dummy_rag_connector_b_robot_1.launch b/launch/cell-sync/dummy_rag_connector_b_robot_1.launch deleted file mode 100644 index 29671f59de6b09219de77c41d29cf4935fb1aec6..0000000000000000000000000000000000000000 --- a/launch/cell-sync/dummy_rag_connector_b_robot_1.launch +++ /dev/null @@ -1,7 +0,0 @@ -<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> diff --git a/launch/cell-sync/dummy_rag_connector_b_robot_2.launch b/launch/cell-sync/dummy_rag_connector_b_robot_2.launch deleted file mode 100644 index 59bd68c60f511bca7a1fe74fdf0bd29f64fe09fa..0000000000000000000000000000000000000000 --- a/launch/cell-sync/dummy_rag_connector_b_robot_2.launch +++ /dev/null @@ -1,7 +0,0 @@ -<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> diff --git a/launch/cleaning/cleaning_dummy_cgv.launch b/launch/cleaning/cleaning_dummy_cgv.launch deleted file mode 100644 index 3632dc3477f6f97e4ad57ce7977c6442a1041c6e..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_dummy_cgv.launch +++ /dev/null @@ -1,3 +0,0 @@ -<launch> - <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" /> -</launch> diff --git a/launch/cleaning/cleaning_dummy_controller.launch b/launch/cleaning/cleaning_dummy_controller.launch deleted file mode 100644 index 7ce2c9c6d21d609514c386376aad87e93c6b1ebe..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_dummy_controller.launch +++ /dev/null @@ -1,11 +0,0 @@ -<launch> - - <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> diff --git a/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch b/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch deleted file mode 100644 index 0caf4848393fd80c6b1d78b9aad651df2db7de1b..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_dummy_controller_and_dummy_cgv.launch +++ /dev/null @@ -1,4 +0,0 @@ -<launch> - <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" /> - <include file="$(find ccf)/launch/cleaning/cleaning_dummy_controller.launch" /> -</launch> diff --git a/launch/cleaning/cleaning_dummy_main_dummy_client_dummy_cgv.launch b/launch/cleaning/cleaning_dummy_main_dummy_client_dummy_cgv.launch deleted file mode 100644 index 8bb339e9dab05f69708e79b02bad4883d7eb75b1..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_dummy_main_dummy_client_dummy_cgv.launch +++ /dev/null @@ -1,11 +0,0 @@ -<launch> - <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" /> - <node pkg="ccf" type="dummy_cgv_controller" name="main_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="client_controller" output="screen"> - <param name="connection_address" type="string" value="tcp://*:6586" /> - <param name="client_controllers" type="yaml" value="[]" /> - </node> -</launch> diff --git a/launch/cleaning/cleaning_dummy_main_simulated_client.launch b/launch/cleaning/cleaning_dummy_main_simulated_client.launch deleted file mode 100644 index 8ccce6177645371fe60c722888599e7f0f68d7c6..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_dummy_main_simulated_client.launch +++ /dev/null @@ -1,13 +0,0 @@ -<launch> - <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/> - - <!--<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />--> - <node pkg="ccf" type="moveit_cgv_controller" name="main_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="client_controller" output="screen"> - <param name="connection_address" type="string" value="tcp://*:6586" /> - <param name="client_controllers" type="yaml" value="[]" /> - </node> -</launch> diff --git a/launch/cleaning/cleaning_robot_controller.launch b/launch/cleaning/cleaning_robot_controller.launch deleted file mode 100644 index 206780e84ea419d32aa204f1925490fdcce37c5c..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_robot_controller.launch +++ /dev/null @@ -1,18 +0,0 @@ -<launch> - - <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" > - <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 diff --git a/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch b/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch deleted file mode 100644 index 190cb8536bccc44f0ae5cdae86319f72f29a829b..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_robot_controller_and_dummy_cgv.launch +++ /dev/null @@ -1,4 +0,0 @@ -<launch> - <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" /> - <include file="$(find ccf)/launch/cleaning/cleaning_robot_controller.launch" /> -</launch> diff --git a/launch/cleaning/cleaning_simulated_main_dummy_client.launch b/launch/cleaning/cleaning_simulated_main_dummy_client.launch deleted file mode 100644 index ef9f1ebc13c8c8c2f009287c82772eef8a2a81c5..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_simulated_main_dummy_client.launch +++ /dev/null @@ -1,13 +0,0 @@ -<launch> - <include file="$(find ccf)/launch/cleaning/_simulation_setup.launch"/> - - <!--<node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" />--> - <node pkg="ccf" type="dummy_cgv_controller" name="main_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="moveit_cgv_controller" name="client_controller" output="screen"> - <param name="connection_address" type="string" value="tcp://*:6586" /> - <param name="client_controllers" type="yaml" value="[]" /> - </node> -</launch> diff --git a/launch/cleaning/cleaning_simulation.launch b/launch/cleaning/cleaning_simulation.launch deleted file mode 100644 index f9b5a6cd9961d5da418ce24fc28302166b21ae92..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_simulation.launch +++ /dev/null @@ -1,13 +0,0 @@ -<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" > - <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 diff --git a/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch b/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch deleted file mode 100644 index 96d7379a184345a50379497e10b5cb737fc78e72..0000000000000000000000000000000000000000 --- a/launch/cleaning/cleaning_simulation_and_dummy_cgv.launch +++ /dev/null @@ -1,4 +0,0 @@ -<launch> - <node pkg="ccf" type="dummy_cgv" name="my_dummy_cgv" output="screen" /> - <include file="$(find ccf)/launch/cleaning/cleaning_simulation.launch" /> -</launch> diff --git a/launch/cleaning/_robot_setup.launch b/launch/robot_setup.launch similarity index 96% rename from launch/cleaning/_robot_setup.launch rename to launch/robot_setup.launch index 0e82cf7ea48a91ae8ab862df84b94b3b018ff4fb..43c62f2e5c9941164d5f01582bb4bd0499a0084d 100644 --- a/launch/cleaning/_robot_setup.launch +++ b/launch/robot_setup.launch @@ -34,6 +34,4 @@ <node pkg="ccf" type="scene_controller" name="scene_controller_instance" output="screen" /> - <rosparam file="$(find ccf)/config/config.yaml" command="load" /> - </launch> \ No newline at end of file diff --git a/launch/cleaning/_simulation_setup.launch b/launch/simulation_setup.launch similarity index 89% rename from launch/cleaning/_simulation_setup.launch rename to launch/simulation_setup.launch index ed05f8465eb1b6e3a7ec07719d1e53c09e72b788..9e1fe644182ae769967136867c1a952b0538dc82 100644 --- a/launch/cleaning/_simulation_setup.launch +++ b/launch/simulation_setup.launch @@ -13,6 +13,4 @@ <node pkg="ccf" type="scene_controller" name="scene_controller_instance" output="screen" /> - <rosparam file="$(find ccf)/config/config.yaml" command="load" /> - </launch> \ No newline at end of file diff --git a/proto/connector.proto b/proto/connector.proto index fe23e0b82d6acd6d722190eae190a6c0fe6f8818..f6415d0d40e3370debfdcdba651d1ad94e1893f5 100644 --- a/proto/connector.proto +++ b/proto/connector.proto @@ -48,12 +48,12 @@ message Object { Color color = 6; } -// the scene is stored within the ROS side and sent to the CGV framework +// the scene is stored within the ROS side and sent to clients message Scene { repeated Object objects = 1; } -// the selection is done by the CGV framework and sent to ROS +// the selection is done by a client and sent to ROS message Selection { string id = 1; // the id corresponds to an id of an Object in a Scene } diff --git a/scripts/debug_init_full_scene b/scripts/debug_init_full_scene deleted file mode 100755 index d3a55b20fef23c0f71fbaa81d42e072a4a8aa3cd..0000000000000000000000000000000000000000 --- a/scripts/debug_init_full_scene +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/bash - -rostopic pub -1 /scene_config/collision_table std_msgs/String -f `rospack find ccf`/config_table.yaml -sleep 3 -rostopic pub -1 /scene_config/collision_bin std_msgs/String -f `rospack find ccf`/config_bins.yaml -sleep 3 -rostopic pub -1 /scene_config/collision_objects std_msgs/String -f `rospack find ccf`/config_objects_full.yaml diff --git a/scripts/debug_init_scene b/scripts/debug_init_scene deleted file mode 100755 index 717f901aa578ecf752ffd9cde9cfbaaddc9250ba..0000000000000000000000000000000000000000 --- a/scripts/debug_init_scene +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/bash - -rostopic pub -1 /scene_config/collision_table std_msgs/String -f `rospack find ccf`/config_table.yaml -sleep 3 -rostopic pub -1 /scene_config/collision_bin std_msgs/String -f `rospack find ccf`/config_bin.yaml -sleep 3 -rostopic pub -1 /scene_config/collision_objects std_msgs/String -f `rospack find ccf`/config_objects.yaml diff --git a/scripts/debug_pick_object b/scripts/debug_pick_object deleted file mode 100755 index 39f23a6216ba0993e3ae8eb0ec64eb0c5931aff5..0000000000000000000000000000000000000000 --- a/scripts/debug_pick_object +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/bash - -rostopic pub -1 /robot/pick std_msgs/String "object1" - diff --git a/scripts/debug_place_object b/scripts/debug_place_object deleted file mode 100755 index 68802304f3c680454849525bb0af21ffe5ea2e69..0000000000000000000000000000000000000000 --- a/scripts/debug_place_object +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -rostopic pub -1 /robot/place std_msgs/String "bin" diff --git a/src/controller/DummyRobotArmController.cpp b/src/controller/DummyRobotArmController.cpp index 55a1825a8dc5169369eca3d58ce54906d5d718e3..da784754a3b41064389188adba55f5b926696188 100644 --- a/src/controller/DummyRobotArmController.cpp +++ b/src/controller/DummyRobotArmController.cpp @@ -10,7 +10,7 @@ bool DummyRobotArmController::pickAndDrop(Object &robot, Object &object, Object &bin, bool simulateOnly) { - ROS_INFO_STREAM("[ros2cgv-dummy] \"Picking and dropping\" for 3 seconds..."); + ROS_INFO_STREAM("[DummyRobotArmController] \"Picking and dropping\" for 3 seconds..."); bool result = RobotArmController::pickAndDrop(robot, object, bin, simulateOnly); ros::Rate(ros::Duration(3)).sleep(); sendScene(); @@ -18,7 +18,7 @@ bool DummyRobotArmController::pickAndDrop(Object &robot, Object &object, Object } bool DummyRobotArmController::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { - ROS_INFO_STREAM("[ros2cgv-dummy] \"Picking and placing\" for 3 seconds..."); + ROS_INFO_STREAM("[DummyRobotArmController] \"Picking and placing\" for 3 seconds..."); bool result = RobotArmController::pickAndPlace(robot, object, location, simulateOnly); ros::Rate(ros::Duration(3)).sleep(); return result; diff --git a/src/controller/MoveItRobotArmController.cpp b/src/controller/MoveItRobotArmController.cpp index 1ba6e3af95701a0051d862b20da095bca8786db2..3f05cee1cab254ef83b5208e550eb8e2fa3f2071 100644 --- a/src/controller/MoveItRobotArmController.cpp +++ b/src/controller/MoveItRobotArmController.cpp @@ -152,7 +152,7 @@ MoveItRobotArmController::MoveItRobotArmController(ros::NodeHandle &nodeHandle, bin_check_client = nodeHandle.serviceClient<ccf::BinCheck>("/scene_controller/check_bin_service"); get_scene_service = nodeHandle.advertiseService<ccf::SceneUpdateRequest, ccf::SceneUpdateResponse>( - "updateCgvScene", + "updateCcfScene", [this](auto &req, auto &res) { ROS_INFO_STREAM("[MoveItRobotArmController] Received a scene update from the controller."); updateScene(req); diff --git a/src/controller/RobotArmController.cpp b/src/controller/RobotArmController.cpp index 9b39591609db4cc91cf15a112e7e5b602ce1255d..d68bf18d82cd75599486d41b37a45e52644ebfb1 100644 --- a/src/controller/RobotArmController.cpp +++ b/src/controller/RobotArmController.cpp @@ -14,7 +14,7 @@ using CetiRosToolbox::getParameter; void RobotArmController::receive(const std::string &channel, const std::string &data) { - ROS_WARN_STREAM("[ros2cgv] Received message on channel " << channel << "."); + ROS_WARN_STREAM("[RobotArmController] Received message on channel " << channel << "."); if (channel == selectionTopic) { Selection selection; selection.ParseFromString(data); @@ -28,7 +28,7 @@ void RobotArmController::receive(const std::string &channel, const std::string & pickPlace.ParseFromString(data); pickPlaceAction(pickPlace); } else { - ROS_WARN_STREAM("[ros2cgv] Received ignored message on channel " << channel << ": " << data); + ROS_WARN_STREAM("[RobotArmController] Received ignored message on channel " << channel << ": " << data); } } @@ -44,12 +44,12 @@ Object *RobotArmController::resolveObject(const std::string &id) { void RobotArmController::sendScene() { if (scene) { // meaning if the (smart) pointer is not a nullptr - ROS_INFO_STREAM("[ros2cgv] Sending scene with " << scene->objects().size() << " objects."); + ROS_INFO_STREAM("[RobotArmController] Sending scene with " << scene->objects().size() << " objects."); sendToAll(sendSceneTopic, scene->SerializeAsString()); sceneUpdateAction(); } else { - ROS_WARN_STREAM("[ros2cgv] Scene is not initialized yet. not sending it."); + ROS_WARN_STREAM("[RobotArmController] Scene is not initialized yet. not sending it."); } } @@ -57,7 +57,7 @@ bool RobotArmController::removeObject(const Object &object) { for (auto it = scene->mutable_objects()->begin(); it != scene->mutable_objects()->end(); ++it) { if (it->id() == object.id()) { - ROS_INFO_STREAM("[ros2cgv] Erasing " << it->id() << " from scene."); + ROS_INFO_STREAM("[RobotArmController] Erasing " << it->id() << " from scene."); scene->mutable_objects()->erase(it); return true; } @@ -71,9 +71,9 @@ bool RobotArmController::pickAndDrop(Object &robot, Object &object, Object &bin, void RobotArmController::initScene(const Scene &newScene) { if (scene) { // meaning if the (smart) pointer is not a nullptr - ROS_INFO_STREAM("[ros2cgv] Resetting scene."); + ROS_INFO_STREAM("[RobotArmController] Resetting scene."); } else { - ROS_INFO_STREAM("[ros2cgv] Initializing scene."); + ROS_INFO_STREAM("[RobotArmController] Initializing scene."); } scene = std::make_unique<Scene>(newScene); ROS_INFO_STREAM("The new scene has " << scene->objects().size() << " objects."); @@ -132,7 +132,7 @@ void RobotArmController::loadScene(const std::string &sceneFile) { std::ifstream t(sceneFile); if (!t.is_open()) { - ROS_ERROR_STREAM("[ros2cgv-dummy] Unable to open scene config file " << sceneFile); + ROS_ERROR_STREAM("[RobotArmController] Unable to open scene config file " << sceneFile); } std::string str; @@ -147,17 +147,17 @@ void RobotArmController::loadScene(const std::string &sceneFile) { Scene newScene; google::protobuf::util::Status status = google::protobuf::util::JsonStringToMessage(str, &newScene); if (!status.ok()) { - ROS_ERROR_STREAM("[ros2cgv-dummy] Unable to parse Json String: " << status.ToString()); + ROS_ERROR_STREAM("[RobotArmController] Unable to parse Json String: " << status.ToString()); } else { - ROS_INFO_STREAM("[ros2cgv-dummy] Parsed a scene with " << newScene.objects().size() << " objects."); + ROS_INFO_STREAM("[RobotArmController] Parsed a scene with " << newScene.objects().size() << " objects."); initScene(newScene); } std::string s; if (google::protobuf::TextFormat::PrintToString(newScene, &s)) { - ROS_DEBUG_STREAM("[ros2cgv-dummy] Received scene" << std::endl << s); + ROS_DEBUG_STREAM("[RobotArmController] Received scene" << std::endl << s); } else { - ROS_WARN_STREAM("[ros2cgv-dummy] Scene invalid! partial content: " << newScene.ShortDebugString()); + ROS_WARN_STREAM("[RobotArmController] Scene invalid! partial content: " << newScene.ShortDebugString()); } } diff --git a/src/util/scene_controller_node.cpp b/src/util/scene_controller_node.cpp index c2654747a09f06d19a38fba703ce69c0ffd102be..c491346660f3f60009a9f7505061843fbd053b9f 100644 --- a/src/util/scene_controller_node.cpp +++ b/src/util/scene_controller_node.cpp @@ -107,9 +107,9 @@ int getNumOfBins() { } /** - * Constructs an scene-update-object and sends it via nng to the cgv-framework. - * @param n a valid nodehandle - * @return true on successfull transmission + * Constructs an scene-update-object and updates it in the CCF controller. + * @param n a valid node handle + * @return true on successful transmission */ bool pushSceneUpdate(ros::NodeHandle &n) { @@ -160,7 +160,7 @@ bool pushSceneUpdate(ros::NodeHandle &n) { req.transform.z = world_state::current_transform.z; - ros::ServiceClient client = n.serviceClient<ccf::SceneUpdate>("/connector_node_ros_cgv/updateCgvScene"); + ros::ServiceClient client = n.serviceClient<ccf::SceneUpdate>("/connector_node_ros_ccf/updateCcfScene"); ccf::SceneUpdate srv; srv.request = req; return client.call(srv); @@ -185,7 +185,7 @@ bool configTable(Scene &table_scene, moveit::planning_interface::PlanningSceneIn } /** - * Configures the bins and triggers an update to the cgv framework. + * Configures the bins and triggers an update in the CCF controller. * @param planning_scene_interface a valid PlanningSceneInterface instance * @param n a valid nodehandle */ @@ -290,7 +290,7 @@ Scene applyInputTransformation(const Scene &scene) { /** - * Configures and updates the objects to pick/place and triggers an update to the cgv framework. + * Configures and updates the objects to pick/place and triggers an update in the CCF controller. * @param msg a message containing the scene * @param planning_scene_interface a valid PlanningSceneInterface instance * @param n a valid nodehandle