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

initial commit of extracted package

parents
No related branches found
No related tags found
No related merge requests found
Showing
with 854 additions and 0 deletions
.idea
cmake-build-*
doc
cmake_minimum_required(VERSION 3.0.2)
project(ccf_immersive_sorting)
## compile as C++14
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
roslib
ccf
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES ccf_immersive_sorting
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/ccf_immersive_sorting.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
set_target_properties(${PROJECT_NAME}_dummy_selection_provider PROPERTIES OUTPUT_NAME dummy_selection_provider PREFIX "")
set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "")
set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_dummy_selection_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_dummy_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_moveit_sorting_controller
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_dummy_selection_provider
${catkin_LIBRARIES}
nng
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ccf_immersive_sorting.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
LICENSE 0 → 100644
BSD 3-Clause License
Copyright (c) 2020, TU Dresden, Software Technology Group
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
{ '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 } }
] }
<launch>
<arg name="connection_address" default="tcp://*:6576" />
<arg name="client_controllers" default="[]" />
<node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="my_dummy_sorting_controller" output="screen">
<param name="connection_address" type="string" value="$(arg connection_address)" />
<param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
<param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
</node>
</launch>
<launch>
<node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
<node pkg="ccf_immersive_sorting" type="dummy_sorting_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']" />
<param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
</node>
<node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="client_controller" output="screen">
<param name="connection_address" type="string" value="tcp://*:6586" />
<param name="client_controllers" type="yaml" value="[]" />
<param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
</node>
</launch>
<launch>
<node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
<include file="$(find ccf_immersive_sorting)/launch/dummy_main.launch" />
</launch>
<launch>
<include file="$(find ccf)/launch/simulation_setup.launch"/>
<node pkg="ccf_immersive_sorting" type="moveit_sorting_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']" />
<param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
</node>
<node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="client_controller" output="screen">
<param name="connection_address" type="string" value="tcp://*:6586" />
<param name="client_controllers" type="yaml" value="[]" />
<param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
</node>
</launch>
<launch>
<node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
</launch>
<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/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_sorting_controller" name="moveit_sorting_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>
<node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
<include file="robot_main.launch" />
</launch>
<launch>
<arg name="connection_address" default="tcp://*:6576" />
<arg name="client_controllers" default="[]" />
<include file="$(find ccf)/launch/simulation_setup.launch"/>
<node pkg="ccf" type="moveit_sorting_controller" name="moveit_sorting_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>
<include file="$(find ccf)/launch/simulation_setup.launch"/>
<node pkg="ccf_immersive_sorting" type="dummy_sorting_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_immersive_sorting" type="moveit_sorting_controller" name="client_controller" output="screen">
<param name="connection_address" type="string" value="tcp://*:6586" />
<param name="client_controllers" type="yaml" value="[]" />
</node>
</launch>
<launch>
<node pkg="ccf_immersive_sorting" type="dummy_selection_provider" name="my_dummy_selection_provider" output="screen"/>
<include file="simulated_main.launch" />
</launch>
<?xml version="1.0"?>
<package format="2">
<name>ccf_immersive_sorting</name>
<version>1.0.0</version>
<description>Immersive Sorting Demo using the CeTI Component Framework</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ccf_immersive_sorting</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<author email="johannes.mey@tu-dresden.de">Johannes Mey</author>
<author email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</author>
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<depend>roscpp</depend>
<depend>ccf</depend>
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
/*! \file dummy_selection_provider.cpp
A ROS node that simulates a selection provider like the CGV framework
\author Johannes Mey
\date 07.07.20
*/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <nng/nng.h>
#include <nng/protocol/pair1/pair.h>
#include <google/protobuf/text_format.h>
#include <random>
#include "connector.pb.h"
#include <google/protobuf/util/json_util.h>
#include <google/protobuf/message.h>
const std::string URL = "tcp://127.0.0.1:6576";
nng_socket sock;
int rv;
void sendSelection(const std::string &object) {
Selection selection;
selection.set_id(object);
int length = selection.ByteSize();
void *data = nng_alloc(length);
selection.SerializeToArray(data, length);
if ((rv = nng_send(sock, data, length, NNG_FLAG_ALLOC)) != 0) {
ROS_ERROR_STREAM("nng_send returned: " << nng_strerror(rv));
}
}
std::string ProtoToJson(const google::protobuf::Message &proto) {
std::string json;
google::protobuf::util::MessageToJsonString(proto, &json);
return json;
}
int main(int argc, char **argv) {
GOOGLE_PROTOBUF_VERIFY_VERSION;
ros::init(argc, argv, "dummy_selection_provider");
ros::NodeHandle n("ccf");
if ((rv = nng_pair1_open(&sock)) != 0) {
ROS_ERROR_STREAM("[dummy_selection_provider] nng_pair1_open returned: " << nng_strerror(rv));
}
ros::Rate connection_retry_rate(1);
while ((rv = nng_dial(sock, URL.c_str(), nullptr, 0)) != 0) {
ROS_WARN_STREAM(
"[dummy_selection_provider] nng_dial returned: " << nng_strerror(rv)
<< ". Trying to connect again in one second...");
connection_retry_rate.sleep();
}
ROS_INFO_STREAM(
"[dummy_selection_provider] nng_dial returned: " << nng_strerror(rv)
<< " (which is the translation of error code " << rv
<< "). Connection established!");
ros::Rate loop_rate(200);
ros::Rate pause_rate(ros::Duration(2)); // seconds
// initialize random number generator
std::random_device rd;
std::mt19937 rng(rd());
while (ros::ok()) {
char *buf = nullptr;
size_t sz;
if ((rv = nng_recv(sock, &buf, &sz, NNG_FLAG_ALLOC | NNG_FLAG_NONBLOCK)) == 0) {
// store the result in a protobuf object and free the buffer
Scene scene;
scene.ParseFromArray(buf, sz);
nng_free(buf, sz);
std::string s;
if (google::protobuf::TextFormat::PrintToString(scene, &s)) {
ROS_INFO_STREAM("[dummy_selection_provider] Received a valid scene with " << scene.objects().size()
<< " objects.");
ROS_DEBUG_STREAM("[dummy_selection_provider] Content:" << std::endl << s);
} else {
ROS_WARN_STREAM("[dummy_selection_provider] Received an invalid scene!" << std::endl
<< "[dummy_selection_provider] Partial content:"
<< std::endl
<< scene.ShortDebugString());
}
// collect all object names
std::vector<std::string> objects{};
std::vector<std::string> bins{};
for (const auto &object : scene.objects()) {
ROS_INFO_STREAM(
"[dummy_selection_provider] found object " << object.id() << " of type "
<< Object_Type_Name(object.type()));
if (object.type() == Object_Type_BOX) {
objects.push_back(object.id());
} else if (object.type() == Object_Type_BIN) {
bins.push_back(object.id());
}
}
if (objects.empty()) {
ROS_INFO("[dummy_selection_provider] No objects to select in a scene without boxes!");
} else if (bins.empty()) {
ROS_INFO("[dummy_selection_provider] No bin to put the object in available!");
} else {
// select a random object
std::uniform_int_distribution<u_long> distribution{0, objects.size() - 1};
// wait some time, then send a selected object
pause_rate.sleep();
std::string object{objects[distribution(rng)]};
ROS_INFO_STREAM("[dummy_selection_provider] Selecting random object: " << object);
sendSelection(object);
// wait again, then send the bin object
pause_rate.sleep();
// we expect an object named "bin" to be there
// select a random object
std::uniform_int_distribution<u_long> binDistribution{0, bins.size() - 1};
std::string bin{bins[binDistribution(rng)]};
ROS_INFO_STREAM("[dummy_selection_provider] Selecting random bin: " << bin);
sendSelection(bin);
}
} else if (rv == NNG_EAGAIN) {
// no message received in current spin
} else {
ROS_ERROR_STREAM("[dummy_selection_provider] nng_recv returned: " << nng_strerror(rv));
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
\ No newline at end of file
/*! \file dummy_sorting_controller.cpp
A ROS node that simulates a node connected to a model-based cobotic application
\author Johannes Mey
\author Sebastian Ebert
\date 07.07.20
*/
#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Empty.h>
#include "connector.pb.h"
#include "ccf/controller/DummyRobotArmController.h"
#include "ccf/connection/NngConnection.h"
#include "ccf/util/NodeUtil.h"
std::string NODE_NAME = "dummy_sorting_controller";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
int main(int argc, char **argv) {
GOOGLE_PROTOBUF_VERIFY_VERSION;
ros::init(argc, argv, NODE_NAME);
NODE_NAME = ros::this_node::getName();
ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
DummyRobotArmController connector{n, NODE_NAME};
// add an NNG connection
std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress);
connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
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"));
client_connection->setReceiveTopic("client_scene");
connector.addConnection(std::move(client_connection));
}
connector.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene.yaml"));
Object *robot = nullptr;
Object *selectedBox = nullptr;
Object *selectedBin = nullptr;
std::optional<std::string> currentlyPickedBox{};
ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector](
const std_msgs::EmptyConstPtr &msg) { connector.sendScene(); });
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, &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;
}
Object *object;
try {
object = connector.resolveObject(selection.id());
} catch (std::out_of_range &e) {
ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
return;
}
auto type = Object::Type_Name(object->type());
if (object->type() == Object::BOX) {
selectedBox = object;
ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
} else if (object->type() == Object::BIN) {
selectedBin = object;
ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
} else {
ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
}
if (selectedBin && selectedBox) {
auto boxId = selectedBox->id();
ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
currentlyPickedBox = boxId;
if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
selectedBox = nullptr;
selectedBin = nullptr;
} else {
ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
selectedBox = nullptr;
selectedBin = nullptr;
connector.sendScene();
}
}
};
connector.reactToSelectionMessage(selectionMessageCallback);
auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value()) {
try {
connector.resolveObject(currentlyPickedBox.value());
} catch (std::out_of_range &e) {
ROS_INFO_STREAM(
"[" << NODE_NAME << "] box " << currentlyPickedBox.value()
<< " has been removed from the scene!");
currentlyPickedBox.reset();
}
}
};
connector.reactToSceneUpdateMessage(sceneUpdateCallback);
ros::spin();
return 0;
}
\ No newline at end of file
/*! \file moveit_sorting_controller.cpp
A ROS node that controls a robot connected to a model-based cobotic application
\author Johannes Mey
\author Sebastian Ebert
\date 07.07.20
*/
#define BOOST_BIND_GLOBAL_PLACEHOLDERS // fix boost
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include "connector.pb.h"
#include "ccf/controller/MoveItRobotArmController.h"
#include "ccf/connection/NngConnection.h"
#include "ccf/util/NodeUtil.h"
std::string NODE_NAME = "moveit_sorting_controller";
using CetiRosToolbox::getParameter;
using CetiRosToolbox::getPrivateParameter;
int main(int argc, char **argv) {
GOOGLE_PROTOBUF_VERIFY_VERSION;
ros::init(argc, argv, NODE_NAME);
NODE_NAME = ros::this_node::getName();
ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
auto connectionAddress = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
MoveItRobotArmController connector{n, NODE_NAME};
// add an NNG connection
std::unique_ptr<NngConnection> connection = std::make_unique<NngConnection>(connectionAddress);
connection->setReceiveTopic(getParameter<std::string>(n, "topics/selection", "selection"));
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"));
client_connection->setReceiveTopic("client_scene");
connector.addConnection(std::move(client_connection));
}
// scene loading is not required, the scene is updated by moveit
Object *robot = nullptr;
Object *selectedBox = nullptr;
Object *selectedBin = nullptr;
std::optional<std::string> currentlyPickedBox{};
ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&connector](
const std_msgs::EmptyConstPtr &msg) { connector.sendScene(); });
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, &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;
}
Object *object;
try {
object = connector.resolveObject(selection.id());
} catch (std::out_of_range &e) {
ROS_ERROR_STREAM("[" << NODE_NAME << "] Selected unknown object '" << selection.id() << "'");
return;
}
auto type = Object::Type_Name(object->type());
if (object->type() == Object::BOX) {
selectedBox = object;
ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
} else if (object->type() == Object::BIN) {
selectedBin = object;
ROS_INFO_STREAM("[" << NODE_NAME << "] Selected " << type << " '" << selection.id() << "'");
} else {
ROS_WARN_STREAM("[" << NODE_NAME << "] Selected unsupported " << type << " '" << selection.id() << "'");
}
if (selectedBin && selectedBox) {
auto boxId = selectedBox->id();
ROS_INFO_STREAM("[" << NODE_NAME << "] Got task: Put " << boxId << " into " << selectedBin->id());
currentlyPickedBox = boxId;
if (!connector.pickAndDrop(*robot, *selectedBox, *selectedBin, false)) {
ROS_WARN_STREAM("[" << NODE_NAME << "] Unable to remove box '" << boxId << "'!");
selectedBox = nullptr;
selectedBin = nullptr;
} else {
ROS_INFO_STREAM("[" << NODE_NAME << "] Job is done! '" << boxId << "' is no more.");
selectedBox = nullptr;
selectedBin = nullptr;
connector.sendScene();
}
}
};
connector.reactToSelectionMessage(selectionMessageCallback);
auto sceneUpdateCallback = [&currentlyPickedBox, &connector]() {
if (currentlyPickedBox.has_value()) {
try {
connector.resolveObject(currentlyPickedBox.value());
} catch (std::out_of_range &e) {
ROS_INFO_STREAM(
"[" << NODE_NAME << "] box " << currentlyPickedBox.value()
<< " has been removed from the scene!");
currentlyPickedBox.reset();
}
}
};
connector.reactToSceneUpdateMessage(sceneUpdateCallback);
ros::spin();
return 0;
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment