robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

Merge branch 'main' into 77-cartesian-controllers

Browse source
This commit is contained in:
Ilya Uraev 2024-01-13 01:49:08 +03:00
commit 2755209c66
144 changed files with 100976 additions and 4303 deletions
Download patch file Download diff file Expand all files Collapse all files

16
.gitlab-ci.yml
View file

@ -13,15 +13,21 @@ build-colcon-job:
stage: build
script:
- apt-get update
- apt-get install -y make wget libgoogle-glog-dev libreadline-dev
- wget http://www.lua.org/ftp/lua-5.3.1.tar.gz && tar zxf lua-5.3.1.tar.gz
- make -C lua-5.3.1 linux && make -C lua-5.3.1 install
- apt-get install wget
- wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz
- tar -xf v3.11.3.tar.gz
- cd json-3.11.3
- mkdir build
- cd build
- cmake ..
- make install
- cd ../..
- mkdir -p .src/robossembler-ros2
- mv * .src/robossembler-ros2
- mv .git .src/robossembler-ros2
- mv .src src
- vcs import src < src/robossembler-ros2/rbs.repos
- vcs import src < src/robossembler-ros2/rbs.sim.repos
- rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
- colcon build --packages-skip cartesian_controller_simulation cartesian_controller_tests --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
- colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
rules:
- if: $CI_COMMIT_REF_NAME != $CI_DEFAULT_BRANCH

46
Dockerfile Normal file
View file

@ -0,0 +1,46 @@
FROM billf1nger/ros2-humble-cuda:v12.2.2-cudnn8-ubuntu22.04-gz-nvidia
ARG WSDIR=rbs_ws
ARG config_type=sim
ENV RBS_ASSEMBLY_DIR=/assembly
# COPY /home/bill-finger/assembly /assembly
ENV IGN_GAZEBO_RESOURCE_PATH=/${WSDIR}/install/rbs_simulation/share/rbs_simulation/
RUN apt update && apt install -y \
git \
python3-pip \
lsb-release \
wget \
gnupg
RUN pip install vcstool
WORKDIR /libs
RUN wget https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz &&\
tar -xf v3.11.3.tar.gz &&\
cd json-3.11.3 &&\
mkdir build &&\
cd build &&\
cmake .. &&\
make &&\
make install
RUN git clone https://gitlab.com/robossembler/forks/megapose6d.git &&\
cd megapose6d &&\
pip install bokeh joblib pin torch transforms3d webdataset omegaconf tqdm &&\
pip install -e .
RUN git clone https://github.com/thodan/bop_toolkit &&\
cd bop_toolkit &&\
pip install -e .
WORKDIR /${WSDIR}
COPY . src/robossembler-ros2/
RUN vcs import src/. < src/robossembler-ros2/rbs.${config_type}.repos
RUN apt update && rosdep update && \
rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
RUN . /opt/ros/humble/setup.sh && \
colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=1

17
README.md
View file

@ -6,6 +6,7 @@ Repo for ROS2 packages related to Robossembler
### Requirements
* OS: Ubuntu 22.04
* ROS 2 Humble
* nlohman/json
### Dependencies
These are the primary dependencies required to use this project.
@ -13,6 +14,7 @@ These are the primary dependencies required to use this project.
* MoveIt 2
> Install/build a version based on the selected ROS 2 release
* Gazebo Fortress
* [nlohman/json](https://github.com/nlohmann/json/releases/tag/v3.11.3)
### Build
@ -24,12 +26,12 @@ Prepare workspace & install dependencies (So far only tested with UR robot arm)
```bash
mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src
git clone https://gitlab.com/robosphere/robossembler-ros2
vcs import . < robossembler-ros2/rbs.repos
vcs import . < robossembler-ros2/rbs.sim.repos
cd ..
rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
```
For simulation with Mujoco, pls [ref](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/blob/fdaa9a2dd0ce15fc949b127dd1164095504273fb/cartesian_controller_simulation/README.md) build and install section.
<!-- For simulation with Mujoco, pls [ref](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/blob/fdaa9a2dd0ce15fc949b127dd1164095504273fb/cartesian_controller_simulation/README.md) build and install section. -->
### Set Gazebo enviroment variables
Replace `[WS_FOLDER]` with your workspace folder
@ -49,16 +51,11 @@ Launch MoveIt2, Gazebo, RViz:
ros2 launch rbs_bringup bringup.launch.py
```
Load Problem file via ROS2 Service (TMP):
Start BT node in another terminal
```
ros2 service call /problem_expert/add_problem plansys2_msgs/srv/AddProblem '{problem: (define (problem box_problem)(:domain atomic_domain)(:objects ur_manipulator_gripper - arm workspace1 - zone box1 box2 box3 - part asm0 asm1 asm2 asm3 - assembly)(:init (part_of box1 asm1)(part_of box2 asm2)(part_of box3 asm3)(assembly_order asm0 asm1)(assembly_order asm1 asm2)(assembly_order asm2 asm3)(arm_available ur_manipulator_gripper)(assembly_at asm0 workspace1)(assembled asm0))(:goal (and (assembled asm3))))}'
```
Start task planner
```
ros2 run rbs_task_planner execute_plan
ros2 launch rbs_bt_executor rbs_executor.launch.py
```
### Links
* [bt_v3_cpp](https://www.behaviortree.dev)
* [moveit2](https://moveit.picknik.ai/humble/index.html)
* [moveit2](https://moveit.picknik.ai/humble/index.html)

68
env_components/camera_component/CMakeLists.txt
View file

@ -1,68 +0,0 @@
cmake_minimum_required(VERSION 3.5)
project(camera_component)
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED True)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_ros REQUIRED)
find_package(image_transport REQUIRED)
find_package(ros_gz_bridge REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(env_manager REQUIRED)
find_package(ignition-transport11 REQUIRED)
set(GZ_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR})
find_package(ignition-msgs8 REQUIRED)
set(GZ_MSGS_VER ${ignition-msgs8_VERSION_MAJOR})
set(GZ_TARGET_PREFIX ignition)
add_library(ign_camera_component SHARED
src/ign_camera_component.cpp)
target_compile_definitions(ign_camera_component
PRIVATE "COMPOSITION_BUILDING_DLL")
target_link_libraries(ign_camera_component
${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core
${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core
)
ament_target_dependencies(ign_camera_component
"image_transport"
"ros_gz_bridge"
"rclcpp"
"rclcpp_components"
"sensor_msgs"
"env_manager")
rclcpp_components_register_nodes(ign_camera_component "camera_component::IgnCameraComponent")
set(node_plugins "${node_plugins}camera_component::IgnCameraComponent;$<TARGET_FILE:camera_component>\n")
install(TARGETS ign_camera_component
DESTINATION lib)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

30
env_components/camera_component/package.xml
View file

@ -1,30 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>camera_component</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>pkg-config</buildtool_depend>
<depend>image_transport</depend>
<depend>ros_gz_bridge</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
<depend>env_manager</depend>
<depend condition="$GZ_VERSION == fortress or $IGNITION_VERSION == fortress">ignition-msgs8</depend>
<depend condition="$GZ_VERSION == fortress or $IGNITION_VERSION == fortress">ignition-transport11</depend>
<depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-msgs8</depend>
<depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-transport11</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

42
env_components/camera_component/src/ign_camera_component.cpp
View file

@ -1,42 +0,0 @@
#include <component_manager/publisher_component.hpp>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <ignition/transport/Node.hh>
#include <rclcpp/rclcpp.hpp>
#include <image_transport/image_transport.hpp>
#include <ros_gz_bridge/convert.hpp>
#include <sensor_msgs/msg/image.hpp>
namespace camera_component
{
class IgnCameraComponent: public env_manager::component_manager::PublisherComponent<sensor_msgs::msg::Image>
{
public:
explicit IgnCameraComponent(const rclcpp::NodeOptions &options)
: env_manager::component_manager::PublisherComponent<sensor_msgs::msg::Image>(options)
{
auto topic_name = "/camera";
_ign_node = std::make_shared<ignition::transport::Node>();
_ign_node->Subscribe(topic_name, &IgnCameraComponent::on_image, this);
}
private:
void on_image(const ignition::msgs::Image& img)
{
sensor_msgs::msg::Image rimg;
ros_gz_bridge::convert_gz_to_ros(img, rimg);
populate_publication(rimg);
}
std::shared_ptr<ignition::transport::Node> _ign_node;
};
} // namespace pub_component
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(camera_component::IgnCameraComponent)

16
env_components/component_interfaces/CMakeLists.txt
View file

@ -1,16 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(component_interfaces)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/DetectObject.srv"
DEPENDENCIES std_msgs)
ament_package()

10
env_components/component_interfaces/srv/DetectObject.srv
View file

@ -1,10 +0,0 @@
uint8 DETECT=0
uint8 FOLLOW=1
uint8 CANCEL=2
string object_name
uint8 req_kind
---
bool call_status
string error_msg

118
env_components/scene_component/CMakeLists.txt
View file

@ -1,118 +0,0 @@
cmake_minimum_required(VERSION 3.5)
project(scene_component)
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED True)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_ros REQUIRED)
find_package(image_transport REQUIRED)
find_package(ros_gz_bridge REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(env_manager REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(component_interfaces REQUIRED)
find_package(ignition-transport11 REQUIRED)
set(GZ_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR})
find_package(ignition-msgs8 REQUIRED)
set(GZ_MSGS_VER ${ignition-msgs8_VERSION_MAJOR})
set(GZ_TARGET_PREFIX ignition)
message(STATUS "Compiling against Ignition Fortress")
add_library(ign_tf2_broadcaster_component SHARED
src/ign_tf2_broadcaster_component.cpp)
target_compile_definitions(ign_tf2_broadcaster_component
PRIVATE "COMPOSITION_BUILDING_DLL")
target_link_libraries(ign_tf2_broadcaster_component
${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core
${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core
)
ament_target_dependencies(ign_tf2_broadcaster_component
"ros_gz_bridge"
"rclcpp"
"rclcpp_components"
"env_manager"
"geometry_msgs"
"tf2_ros")
rclcpp_components_register_nodes(ign_tf2_broadcaster_component "scene_component::IgnTf2Broadcaster")
set(node_plugins "${node_plugins}scene_component::IgnTf2Broadcaster;$<TARGET_FILE:scene_component>\n")
add_library(ign_detect_object_component SHARED
src/ign_detect_object_component.cpp)
target_compile_definitions(ign_detect_object_component
PRIVATE "COMPOSITION_BUILDING_DLL")
target_link_libraries(ign_detect_object_component
${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core
${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core
)
ament_target_dependencies(ign_detect_object_component
"ros_gz_bridge"
"rclcpp"
"rclcpp_components"
"env_manager"
"geometry_msgs"
"tf2_ros"
"component_interfaces")
rclcpp_components_register_nodes(ign_detect_object_component "scene_component::IgnDetectOpjectService")
set(node_plugins "${node_plugins}scene_component::IgnDetectObjectService;$<TARGET_FILE:scene_component>\n")
add_library(moveit_scene_component SHARED
src/moveit_scene_component.cpp)
target_compile_definitions(moveit_scene_component
PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(moveit_scene_component
"rclcpp"
"rclcpp_components"
"env_manager"
"geometry_msgs"
"tf2_ros"
"moveit_core"
"moveit_msgs"
"moveit_ros_planning"
"moveit_ros_planning_interface")
rclcpp_components_register_nodes(moveit_scene_component "scene_component::MoveitSceneComponent")
set(node_plugins "${node_plugins}scene_component::MoveitSceneComponent;$<TARGET_FILE:scene_component>\n")
install(
TARGETS
ign_detect_object_component
ign_tf2_broadcaster_component
moveit_scene_component
DESTINATION
lib)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

31
env_components/scene_component/package.xml
View file

@ -1,31 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>scene_component</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>pkg-config</buildtool_depend>
<depend>image_transport</depend>
<depend>ros_gz_bridge</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>geometry_msgs</depend>
<depend>env_manager</depend>
<depend>component_interfaces</depend>
<depend condition="$GZ_VERSION == fortress or $IGNITION_VERSION == fortress">ignition-msgs8</depend>
<depend condition="$GZ_VERSION == fortress or $IGNITION_VERSION == fortress">ignition-transport11</depend>
<depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-msgs8</depend>
<depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-transport11</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

109
env_components/scene_component/src/ign_detect_object_component.cpp
View file

@ -1,109 +0,0 @@
#include "component_manager/service_component.hpp"
#include "component_interfaces/srv/detect_object.hpp"
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_msgs/msg/tf_message.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <ignition/transport/Node.hh>
#include <ros_gz_bridge/convert.hpp>
#define IGN_DEFAULT_BASE_FRAME "world"
#define IGN_DEFAULT_WORLD_NAME "mir"
namespace scene_component
{
class IgnDetectObjectService: public
env_manager::component_manager::ServiceComponent<component_interfaces::srv::DetectObject>
{
public:
IgnDetectObjectService(const rclcpp::NodeOptions &options)
: env_manager::component_manager::ServiceComponent<component_interfaces::srv::DetectObject>(options)
{
std::string ign_topic_name = "/world/" + std::string(IGN_DEFAULT_WORLD_NAME) + "/dynamic_pose/info";
_tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
_ign_node = std::make_shared<ignition::transport::Node>();
_ign_node->Subscribe(ign_topic_name, &IgnDetectObjectService::on_pose, this);
}
void callback(
std::shared_ptr<component_interfaces::srv::DetectObject::Request> request,
std::shared_ptr<component_interfaces::srv::DetectObject::Response> response)
{
auto kind = static_cast<DetectObjectReqKind>(request->req_kind);
bool status = false;
switch(kind)
{
case DETECT: {
(void) kind;
break;
} case FOLLOW: {
auto ok = std::find(_follow_frames.begin(), _follow_frames.end(),
request->object_name) == _follow_frames.end();
if (ok)
_follow_frames.push_back(request->object_name);
status = ok;
break;
} case CANCEL: {
auto it = std::find(_follow_frames.begin(), _follow_frames.end(),
request->object_name);
auto ok = it != _follow_frames.end();
if (ok)
_follow_frames.erase(it);
status = ok;
break;
}
}
response->call_status = status;
}
private:
enum DetectObjectReqKind
{
DETECT=0,
FOLLOW=1,
CANCEL=2
};
void on_pose(const ignition::msgs::Pose_V &pose_v)
{
auto poses = pose_v.pose();
for (const auto& obj: _follow_frames)
{
auto it = std::find_if(poses.begin(), poses.end(),
[&obj](const auto& pose){ return pose.name() == obj;});
if (it == poses.end())
continue;
geometry_msgs::msg::PoseStamped rpose;
ros_gz_bridge::convert_gz_to_ros(*it, rpose);
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = IGN_DEFAULT_BASE_FRAME;
t.child_frame_id = it->name();
t.transform.translation.x = rpose.pose.position.x;
t.transform.translation.y = rpose.pose.position.y;
t.transform.translation.z = rpose.pose.position.z;
t.transform.rotation.x = rpose.pose.orientation.x;
t.transform.rotation.y = rpose.pose.orientation.y;
t.transform.rotation.z = rpose.pose.orientation.z;
t.transform.rotation.w = rpose.pose.orientation.w;
_tf2_broadcaster->sendTransform(t);
}
}
std::vector<std::string> _follow_frames;
std::unique_ptr<tf2_ros::TransformBroadcaster> _tf2_broadcaster;
std::shared_ptr<ignition::transport::Node> _ign_node;
};
} // namespace scene_component
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnDetectObjectService)

72
env_components/scene_component/src/ign_tf2_broadcaster_component.cpp
View file

@ -1,72 +0,0 @@
#include "component_manager/node_component.hpp"
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_msgs/msg/tf_message.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <ignition/transport/Node.hh>
#include <ros_gz_bridge/convert.hpp>
namespace scene_component
{
const std::string DEFAULT_NODE_NAME = "ign_tf2_broadcaster";
const std::string DEFAULT_IGN_WORLD_NAME = "mir";
const std::string DEFAULT_BASE_FRAME_NAME = "world";
class IgnTf2Broadcaster: public env_manager::component_manager::NodeComponent
{
public:
IgnTf2Broadcaster(const rclcpp::NodeOptions & options)
: env_manager::component_manager::NodeComponent(options)
{
_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"};
std::string topic_name = "/world/" + DEFAULT_IGN_WORLD_NAME + "/dynamic_pose/info";
// init broadcaster
_tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
_ign_node = std::make_shared<ignition::transport::Node>();
_ign_node->Subscribe(topic_name, &IgnTf2Broadcaster::on_pose, this);
}
private:
void on_pose(const ignition::msgs::Pose_V& pose_v)
{
for (const auto &it: pose_v.pose())
{
if (std::find(_follow_frames.begin(), _follow_frames.end(), it.name()) == _follow_frames.end())
continue;
geometry_msgs::msg::PoseStamped rpose;
ros_gz_bridge::convert_gz_to_ros(it, rpose);
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = DEFAULT_BASE_FRAME_NAME;
t.child_frame_id = it.name();
t.transform.translation.x = rpose.pose.position.x;
t.transform.translation.y = rpose.pose.position.y;
t.transform.translation.z = rpose.pose.position.z;
t.transform.rotation.x = rpose.pose.orientation.x;
t.transform.rotation.y = rpose.pose.orientation.y;
t.transform.rotation.z = rpose.pose.orientation.z;
t.transform.rotation.w = rpose.pose.orientation.w;
RCLCPP_ERROR(get_logger(), "Provide transform for frame: %s", it.name().c_str());
_tf2_broadcaster->sendTransform(t);
}
}
std::unique_ptr<tf2_ros::TransformBroadcaster> _tf2_broadcaster;
std::shared_ptr<ignition::transport::Node> _ign_node;
std::vector<std::string> _follow_frames;
};
} // namespace scene_component
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnTf2Broadcaster)

115
env_components/scene_component/src/moveit_scene_component.cpp
View file

@ -1,115 +0,0 @@
#include <rclcpp/rclcpp.hpp>
#include <component_manager/node_component.hpp>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/moveit_cpp/planning_component.h>
#include <geometry_msgs/msg/point_stamped.h>
#include <urdf_parser/urdf_parser.h>
#include <geometric_shapes/shape_operations.h>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
using namespace std::chrono_literals;
namespace scene_component
{
const std::string DEFAULT_PLANNING_GOUP_NAME = "panda_arm";
const std::string DEFAULT_SUBNODE_NS = "moveit_cpp_node";
const std::string DEFAULT_BASE_FRAME = "world";
class MoveitSceneComponent: public env_manager::component_manager::NodeComponent
{
public:
MoveitSceneComponent(const rclcpp::NodeOptions& options)
: env_manager::component_manager::NodeComponent(options)
{
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
node_ = rclcpp::Node::make_shared(DEFAULT_SUBNODE_NS, "", node_options);
moveit_cpp_ptr_ = std::make_shared<moveit_cpp::MoveItCpp>(node_);
moveit_cpp_ptr_->getPlanningSceneMonitor()->providePlanningSceneService();
planning_components_ = std::make_shared<moveit_cpp::PlanningComponent>(
DEFAULT_PLANNING_GOUP_NAME, moveit_cpp_ptr_);
robot_model_ptr_ = moveit_cpp_ptr_->getRobotModel();
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
timer_ = this->create_wall_timer(1s, std::bind(&MoveitSceneComponent::on_timer, this));
}
private:
void on_timer()
{
auto available_frames = tf_buffer_->getAllFrameNames();
auto robot_link_names = robot_model_ptr_->getLinkModelNames();
for (const auto &frame: available_frames)
{
if (std::find(robot_link_names.begin(), robot_link_names.end(), frame) != robot_link_names.end())
continue;
geometry_msgs::msg::TransformStamped t;
try {
t = tf_buffer_->lookupTransform(
DEFAULT_BASE_FRAME, frame,
tf2::TimePointZero);
} catch (const tf2::TransformException &ex)
{
RCLCPP_WARN(get_logger(), "Could not transform %s to %s: %s",
frame.c_str(), DEFAULT_BASE_FRAME.c_str(), ex.what());
}
moveit_msgs::msg::CollisionObject co;
co.header.frame_id = robot_model_ptr_->getRootLink()->getName();
co.id = frame;
//TODO: replace with stl model
shape_msgs::msg::SolidPrimitive obj;
obj.type = obj.BOX;
obj.dimensions = {0.1, 0.1, 0.1};
geometry_msgs::msg::Pose obj_pose;
obj_pose.position.x = t.transform.translation.x;
obj_pose.position.y = t.transform.translation.y;
obj_pose.position.z = t.transform.translation.z;
obj_pose.orientation.x = t.transform.rotation.x;
obj_pose.orientation.y = t.transform.rotation.y;
obj_pose.orientation.z = t.transform.rotation.z;
obj_pose.orientation.w = t.transform.rotation.w;
co.primitives.push_back(obj);
co.primitive_poses.push_back(obj_pose);
co.operation = co.ADD;
{
planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr_->getPlanningSceneMonitor());
scene->processCollisionObjectMsg(co);
}
}
}
rclcpp::Node::SharedPtr node_;
std::shared_ptr<moveit_cpp::MoveItCpp> moveit_cpp_ptr_;
moveit::core::RobotModelConstPtr robot_model_ptr_;
std::shared_ptr<moveit_cpp::PlanningComponent> planning_components_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
rclcpp::TimerBase::SharedPtr timer_{nullptr};
};
} // namesapce scene_component
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::MoveitSceneComponent)

144
env_manager/CMakeLists.txt
View file

@ -1,144 +0,0 @@
cmake_minimum_required(VERSION 3.5)
project(env_manager)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED True)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
set(ENV_MANAGER_CONFIGURATION_FILES_DIRECTORY ${PROJECT_SOURCE_DIR}/config
CACHE PATH ".lua configuration files dir")
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_ros REQUIRED)
find_package(Boost REQUIRED)
find_package(rcl REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(rcutils REQUIRED)
find_package(glog 0.4.0 REQUIRED)
find_package(Lua 5.3 REQUIRED)
add_library(env_manager SHARED
src/component_manager/component_manager.cpp
src/config/config_file_resolver.cpp
src/config/lua_file_resolver.cpp
src/config/options.cpp
)
configure_file(
${PROJECT_SOURCE_DIR}/include/config/config.hpp.cmake
${PROJECT_BINARY_DIR}/include/config/config.hpp)
target_include_directories(env_manager PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(
env_manager
Boost
rcl
glog
Lua
class_loader
rclcpp
rclcpp_components
)
target_include_directories(env_manager PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>)
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
target_compile_definitions(env_manager PRIVATE "COMPONENT_MANAGER_BUILDING_LIBRARY")
if(NOT WIN32)
ament_environment_hooks(
"${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}")
endif()
add_executable(env_manager_node
src/main.cpp
)
target_link_libraries(env_manager_node env_manager lua glog)
ament_target_dependencies(
env_manager_node
Boost
rcl
rclcpp
glog
Lua
)
install(DIRECTORY config DESTINATION config)
install(
DIRECTORY include/
DESTINATION include
)
install(
TARGETS env_manager
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(
TARGETS env_manager_node
DESTINATION lib/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_cppcheck_FOUND TRUE)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
set(ament_cmake_flake8_FOUND TRUE)
set(ament_cmake_uncrustify_FOUND TRUE)
set(ament_cmake_pep257_FOUND TRUE)
set(ament_cmake_lint_cmake_FOUND TRUE)
set(ament_cmake_xmllint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
#include_directories(test)
#ament_add_gtest(env_manager_test test/general.cpp)
#target_link_libraries(env_manager_test env_manager lua glog)
#set(TEST_TEST_FILES_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_files)
#set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files)
#install(DIRECTORY
# ${TEST_TEST_FILES_DIR}
# DESTINATION share/${PROJECT_NAME}
#)
endif()
ament_export_include_directories(
include
)
ament_export_libraries(
env_manager
)
ament_export_targets(
export_${PROJECT_NAME}
)
ament_export_dependencies(rcutils)
ament_package()

149
env_manager/README.md
View file

@ -1,149 +0,0 @@
## Environment Manager
Environment Manager is a package that allow you to create custom environments based on *component*, switch between different environments, synchronization of different environments with each other.
![](docs/assets/env_manager.png)
### Getting Started
To get a local copy up and running follow these simple example steps.
#### Prerequisites
```bash
# install glog
sudo apt-get install libgoogle-glog-dev
#install lua
wget http://www.lua.org/ftp/lua-5.3.1.tar.gz
tar zxf lua-5.3.1.tar.gz
sudo make -C lua-5.3.1 linux && sudo make -C lua-5.3.1 install
```
### Config
Configuration is done by lua.
```lua
-- require configuration table
ENV_MANAGER = {
-- dictionary of environments
environments = {
-- environment to upload with manager
example_environment = {
-- namespace in which all components will be
namespace = "<namespace_name>",
-- dictionary of components needed to upload for current environment
components = {
-- node match one of node in ENV_MANAGER.nodes
example_node = {
-- example libpub_component.so
lib = "<library_name>"
-- name of class which implement example_node logic
class = "<class_name>"
}
},
},
},
-- dictionary of nodes, that will be created by env_manager
nodes = {
-- node for initialization by env_manger
example_node = {
-- node name
name = "<node_name>",
-- one node can produce only one topic/service
type = "<Publisher|Subscriber|Service|Client>",
-- example "std_msgs/String",
msg_type = "<msg_type>",
},
},
}
-- return configuration table
return ENV_MANAGER
```
You can safely use all the feature of `lua` to write configuration.
### Usage
Create component library.
```c++
#include "component_manager/publisher_component.hpp"
#include <chrono>
#include <iostream>
#include <memory>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include <std_msgs/msg/string.hpp>
namespace pub_component
{
using namespace std::chrono_literals;
class Publisher: public env_manager::component_manager::PublisherComponent<std_msgs::msg::String>
{
public:
explicit Publisher(const rclcpp::NodeOptions &options)
: env_manager::component_manager::PublisherComponent<std_msgs::msg::String>(options)
{
timer_ = create_wall_timer(1s, std::bind(&Publisher::on_timer, this));
}
protected:
void on_timer()
{
auto msg = std::make_unique<std_msgs::msg::String>();
msg->data = "Hello World!";
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg->data.c_str());
std::flush(std::cout);
this->populate_publication(*msg.get());
}
private:
rclcpp::TimerBase::SharedPtr timer_;
};
} // namespace pub_component
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(pub_component::Publisher)
```
```cmake
add_library(pub_component SHARED
src/pub_component.cpp)
target_compile_definitions(pub_component
PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(pub_component
"rclcpp"
"env_manager"
"rclcpp_components"
"std_msgs")
rclcpp_components_register_nodes(pub_component "pub_component::Publisher")
set(node_plugins "${node_plugins}pub_component::Publisher;$<TARGET_FILE:pub_component>\n")
```
Add component_library to your configuration file.
```lua
ENV_MANAGER = {
environments = {
example_environment = {
namespace = "<namespace_name>",
components = {
example_node = {
lib = "libpub_component.so"
class = "pub_component::Publisher"
}
},
},
},
nodes = {
example_node = {
name = "<node_name>",
type = "Publisher",
msg_type = "std_msgs/String",
},
},
}
return ENV_MANAGER
```
Start env_manager
```bash
source install/setup.bash
ros2 run env_manager env_manager_node
```
### License
Distributed under the Apache 2.0 License. See LICENSE for more information.

19
env_manager/config/config.lua
View file

@ -1,19 +0,0 @@
-- env_manager configuraiton file
include "nodes.lua"
include "utils/utils.lua"
-- include environments configs
include "environments/simulator.lua"
include "environments/ground_true.lua"
-- env_manager configuration
ENV_MANAGER = {
environments = {
GROUND_TRUE
},
nodes = NODES
}
check_nodes(ENV_MANAGER)
return ENV_MANAGER

14
env_manager/config/environments/ground_true.lua
View file

@ -1,14 +0,0 @@
-- environment configuraiton
GROUND_TRUE = {
namespace = "ground_true",
components = {
camera_node = {
lib = "libign_camera_component.so",
class = "camera_component::IgnCameraComponent"
},
scene_component = {
lib = "libign_detect_object_component.so",
class = "scene_component::IgnDetectObjectService"
},
}
}

19
env_manager/config/environments/simulator.lua
View file

@ -1,19 +0,0 @@
-- environment configuration
SIMULATOR = {
namespace = "simulator_env",
components = {
--[[
talker_node = {
lib = "libpub_component.so",
class = "pub_component::Publisher",
},
service_node = {
lib = "libsrv_component.so",
class = "srv_component::Service"
},]]--
camera_node = {
lib = "libign_camera_component.so",
class = "camera_component::IgnCameraComponent"
},
}
}

12
env_manager/config/nodes.lua
View file

@ -1,12 +0,0 @@
NODES = {
camera_node = {
name = "camera_node",
type = "Publisher",
msg_type = "sensor_msgs/Image"
},
scene_component = {
name = "scene_component",
type = "Service",
msg_type = "component_interfaces/DetectObject"
},
}

9
env_manager/config/utils/utils.lua
View file

@ -1,9 +0,0 @@
function check_nodes(config)
for env, cfg in pairs(config.environments) do
for comp, opt in pairs(cfg.components) do
assert(config.nodes[comp] ~= nil, "not all nodes presented.")
assert(opt.lib ~= nil, "not library provided.")
assert(opt.class ~= nil, "not class provided.")
end
end
end

BIN
env_manager/docs/assets/env_manager.png
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 176 KiB

59
env_manager/env_interface/CMakeLists.txt Normal file
View file

@ -0,0 +1,59 @@
cmake_minimum_required(VERSION 3.8)
project(env_interface)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
set(INCLUDE_DEPENDS
rclcpp_lifecycle
rbs_utils
tf2_ros
tf2_eigen
)
# find dependencies
find_package(ament_cmake REQUIRED)
foreach(Dependency IN LISTS INCLUDE_DEPENDS)
find_package(${Dependency} REQUIRED)
endforeach()
add_library(${PROJECT_NAME} SHARED
src/env_interface_base.cpp)
target_include_directories(${PROJECT_NAME} PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/env_interface>
)
ament_target_dependencies(${PROJECT_NAME} PUBLIC ${INCLUDE_DEPENDS})
target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIROMENT_INTERFACE_BUILDING_DLL")
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
install(
DIRECTORY include/
DESTINATION include/env_interface
)
install(TARGETS ${PROJECT_NAME}
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(${INCLUDE_DEPENDS})
ament_package()

4
env_manager/LICENSE → env_manager/env_interface/LICENSE
View file

@ -1,3 +1,4 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
@ -186,7 +187,7 @@
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright 2022 Robossembler
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
@ -199,4 +200,3 @@
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

16
env_manager/env_interface/include/env_interface/env_interface.hpp Normal file
View file

@ -0,0 +1,16 @@
#include "env_interface/env_interface_base.hpp"
#include "rbs_utils/rbs_utils.hpp"
#include <memory>
#include <vector>
namespace env_interface
{
class EnvInterface : public EnvInterfaceBase
{
public:
EnvInterface() = default;
virtual ~EnvInterface() = default;
protected:
std::shared_ptr<std::vector<rbs_utils::EnvModel>> m_env_models;
};
} // namespace env_interface

40
env_manager/env_interface/include/env_interface/env_interface_base.hpp Normal file
View file

@ -0,0 +1,40 @@
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include <cstddef>
#include <cstdint>
namespace env_interface
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// template <typename A_space, typename S_space>
class EnvInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
{
public:
EnvInterfaceBase() = default;
virtual ~EnvInterfaceBase() = default;
virtual CallbackReturn init(const std::string& t_env_name, const std::string& t_namespace = "",
const rclcpp::NodeOptions& t_node_options = rclcpp::NodeOptions());
const rclcpp_lifecycle::State& configure();
virtual CallbackReturn on_init() = 0;
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> getNode();
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> getNode() const;
const rclcpp_lifecycle::State& getState() const;
// virtual void setActionSpace(const A_space& action, const std::size_t& size);
protected:
// A_space action_space;
// S_space observation_space;
private:
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> m_node;
};
} // namespace env_interface

11
env_components/component_interfaces/package.xml → env_manager/env_interface/package.xml
View file

@ -1,18 +1,17 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>component_interfaces</name>
<name>env_interface</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<depend>std_msgs</depend>
<depend>rbs_utils</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>

6
env_manager/env_interface/src/env_interface.cpp Normal file
View file

@ -0,0 +1,6 @@
#include "env_interface/env_interface.hpp"
namespace env_interface {
}

45
env_manager/env_interface/src/env_interface_base.cpp Normal file
View file

@ -0,0 +1,45 @@
#include "env_interface/env_interface_base.hpp"
namespace env_interface
{
CallbackReturn EnvInterfaceBase::init(const std::string& t_env_name, const std::string& t_namespace,
const rclcpp::NodeOptions& t_node_options)
{
m_node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(t_env_name, t_namespace, t_node_options, false);
if (on_init() == CallbackReturn::FAILURE)
{
return CallbackReturn::FAILURE;
}
m_node->register_on_configure(std::bind(&EnvInterfaceBase::on_configure, this, std::placeholders::_1));
m_node->register_on_activate(std::bind(&EnvInterfaceBase::on_activate, this, std::placeholders::_1));
m_node->register_on_cleanup(std::bind(&EnvInterfaceBase::on_cleanup, this, std::placeholders::_1));
m_node->register_on_deactivate(std::bind(&EnvInterfaceBase::on_deactivate, this, std::placeholders::_1));
m_node->register_on_error(std::bind(&EnvInterfaceBase::on_error, this, std::placeholders::_1));
m_node->register_on_shutdown(std::bind(&EnvInterfaceBase::on_shutdown, this, std::placeholders::_1));
return CallbackReturn::SUCCESS;
}
const rclcpp_lifecycle::State& EnvInterfaceBase::configure()
{
return getNode()->configure();
}
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> EnvInterfaceBase::getNode()
{
if (!m_node.get())
{
throw std::runtime_error("Lifecycle node hasn't been initialized yet");
}
return m_node;
}
const rclcpp_lifecycle::State& EnvInterfaceBase::getState() const
{
return m_node->get_current_state();
}
} // namespace env_interface

77
env_manager/env_manager/CMakeLists.txt Normal file
View file

@ -0,0 +1,77 @@
cmake_minimum_required(VERSION 3.8)
project(env_manager)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
set(INCLUDE_DEPENDS
rclcpp
rclcpp_lifecycle
env_manager_interfaces
pluginlib
env_interface
)
# find dependencies
find_package(ament_cmake REQUIRED)
foreach(Dep IN ITEMS ${INCLUDE_DEPENDS})
find_package(${Dep} REQUIRED)
endforeach()
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_library(${PROJECT_NAME} SHARED
src/env_manager.cpp
)
target_include_directories(${PROJECT_NAME} PRIVATE include)
ament_target_dependencies(${PROJECT_NAME} ${INCLUDE_DEPENDS})
target_compile_definitions(${PROJECT_NAME} PRIVATE "ENVIRONMENT_MANAGER_BUILDING_DLL")
add_executable(run_env_manager src/run_env_manager.cpp)
target_include_directories(run_env_manager PRIVATE include)
target_link_libraries(run_env_manager ${PROJECT_NAME})
ament_target_dependencies(run_env_manager ${INCLUDE_DEPENDS})
install(TARGETS ${PROJECT_NAME}
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
)
install(TARGETS run_env_manager
RUNTIME DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/
DESTINATION include
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_export_libraries(
${PROJECT_NAME}
)
ament_export_include_directories(
include
)
ament_export_dependencies(
${INCLUDE_DEPENDS}
)
ament_package()

202
env_manager/env_manager/LICENSE Normal file
View file

@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

100
env_manager/env_manager/include/env_manager/env_manager.hpp Normal file
View file

@ -0,0 +1,100 @@
#ifndef __ENV_MANAGER_HPP__
#define __ENV_MANAGER_HPP__
#include "env_manager_interfaces/srv/configure_env.hpp"
#include "env_manager_interfaces/srv/load_env.hpp"
#include "env_manager_interfaces/srv/unload_env.hpp"
#include "env_manager_interfaces/srv/start_env.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "env_interface/env_interface.hpp"
#include "pluginlib/class_loader.hpp"
namespace env_manager {
using EnvInterface = env_interface::EnvInterface;
using EnvInterfaceSharedPtr = std::shared_ptr<env_interface::EnvInterface>;
using EnvStateReturnType =
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
struct EnvSpec {
std::string name;
std::string type;
EnvInterfaceSharedPtr env_ptr;
};
class EnvManager : public rclcpp::Node {
public:
// EnvManager(rclcpp::Executor::SharedPtr executor,
// const std::string &asm_config,
// const std::string &node_name = "env_manager",
// const std::string &node_namespace = "",
// rclcpp::NodeOptions &node_options =
// rclcpp::NodeOptions()
// .allow_undeclared_parameters(true)
// .automatically_declare_parameters_from_overrides(true));
EnvManager(rclcpp::Executor::SharedPtr executor,
const std::string &node_name = "env_manager",
const std::string &node_namespace = "",
rclcpp::NodeOptions &node_options =
rclcpp::NodeOptions()
.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true));
virtual ~EnvManager() = default;
// EnvInterfaceSharedPtr loadEnv(const std::string& env_name);
EnvInterfaceSharedPtr loadEnv(const std::string &env_name,
const std::string &env_class_type);
EnvStateReturnType unloadEnv(const std::string &env_name);
EnvStateReturnType configureEnv(const std::string &env_name);
EnvInterfaceSharedPtr addEnv(const EnvSpec &enviment);
// TODO: Define the input data format
// Set Target for RL enviroment
EnvInterfaceSharedPtr setTarget();
EnvInterfaceSharedPtr switchTarget();
EnvInterfaceSharedPtr unsetTarget();
// Load Constraints for RL enviroment
EnvInterfaceSharedPtr loadConstraints();
EnvInterfaceSharedPtr switchConstraints();
EnvInterfaceSharedPtr unloadConstraints();
protected:
void initServices();
rclcpp::Node::SharedPtr getNode();
void startEnv_cb(
const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request,
env_manager_interfaces::srv::StartEnv::Response::SharedPtr response);
void loadEnv_cb(
const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response);
void configureEnv_cb(
const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr
request,
env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response);
void unloadEnv_cb(
const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response);
private:
rclcpp::Node::SharedPtr m_node;
std::mutex m_env_mutex;
std::vector<EnvSpec> m_active_envs;
rclcpp::Service<env_manager_interfaces::srv::LoadEnv>::SharedPtr
m_load_env_srv;
rclcpp::Service<env_manager_interfaces::srv::ConfigureEnv>::SharedPtr
m_configure_env_srv;
rclcpp::Service<env_manager_interfaces::srv::UnloadEnv>::SharedPtr
m_unload_env_srv;
rclcpp::Service<env_manager_interfaces::srv::StartEnv>::SharedPtr m_start_env_srv;
rclcpp::CallbackGroup::SharedPtr m_callback_group;
rclcpp::Executor::SharedPtr m_executor;
std::shared_ptr<pluginlib::ClassLoader<env_interface::EnvInterface>> m_loader;
};
} // namespace env_manager
#endif // __ENV_MANAGER_HPP__

23
env_manager/env_manager/package.xml Normal file
View file

@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>env_manager</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ur.narmak@gmail.com">banana</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>env_interface</depend>
<depend>env_manager_interfaces</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

191
env_manager/env_manager/src/env_manager.cpp Normal file
View file

@ -0,0 +1,191 @@
#include "env_manager/env_manager.hpp"
#include "nlohmann/json.hpp"
#include <string>
static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE =
"env_interface";
static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAME =
"env_interface::EnvInterface";
namespace env_manager {
EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
const std::string &node_name,
const std::string &node_namespace,
rclcpp::NodeOptions &options)
: rclcpp::Node(node_name, node_namespace, options), m_executor(executor),
m_loader(
std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>(
ENV_INTERFACE_BASE_CLASS_NAMESPACE,
ENV_INTERFACE_BASE_CLASS_NAME)) {
initServices();
}
// EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
// const std::string &asm_config,
// const std::string &node_name,
// const std::string &node_namespace,
// rclcpp::NodeOptions &options)
// : rclcpp::Node(node_name, node_namespace, options), m_executor(executor),
// m_loader(
// std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>(
// ENV_INTERFACE_BASE_CLASS_NAMESPACE,
// ENV_INTERFACE_BASE_CLASS_NAME)) {
// initServices();
// }
void EnvManager::initServices() {
using namespace std::placeholders;
m_load_env_srv = create_service<env_manager_interfaces::srv::LoadEnv>(
"~/load_env", std::bind(&EnvManager::loadEnv_cb, this, _1, _2));
m_configure_env_srv =
create_service<env_manager_interfaces::srv::ConfigureEnv>(
"~/configure_env",
std::bind(&EnvManager::configureEnv_cb, this, _1, _2));
m_unload_env_srv = create_service<env_manager_interfaces::srv::UnloadEnv>(
"~/unload_env", std::bind(&EnvManager::unloadEnv_cb, this, _1, _2));
m_start_env_srv = create_service<env_manager_interfaces::srv::StartEnv>(
"~/start_env", std::bind(&EnvManager::startEnv_cb, this, _1, _2));
}
rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; }
void EnvManager::startEnv_cb(
const env_manager_interfaces::srv::StartEnv::Request::SharedPtr request,
env_manager_interfaces::srv::StartEnv::Response::SharedPtr response) {
auto it = std::find_if(
m_active_envs.begin(), m_active_envs.end(),
[&request](const EnvSpec &env) { return env.name == request->name; });
if (it != m_active_envs.end()) {
RCLCPP_INFO(get_logger(), "Unloading existing environment '%s'",
request->name.c_str());
unloadEnv(request->name);
}
auto loadedEnv = loadEnv(request->name, request->type);
auto configuredEnv = configureEnv(request->name);
response->ok = (loadedEnv.get() != nullptr &&
configuredEnv == EnvStateReturnType::SUCCESS)
? true
: false;
}
void EnvManager::loadEnv_cb(
const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) {
response->ok = loadEnv(request->name, request->type).get() != nullptr;
}
void EnvManager::configureEnv_cb(
const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
env_manager_interfaces::srv::ConfigureEnv::Response::SharedPtr response) {
response->ok = configureEnv(request->name) == EnvStateReturnType::SUCCESS;
}
void EnvManager::unloadEnv_cb(
const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response) {
response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS;
}
EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) {
// generate list of active enviroments
// std::string unique_env_name = enviroment.name;// + "_" +
// std::to_string(m_active_envs.size()); m_active_envs[unique_env_name] =
// enviroment.env_ptr;
if (enviroment.env_ptr->init(enviroment.name, get_namespace()) ==
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::
CallbackReturn::ERROR) {
RCLCPP_ERROR(get_logger(),
"Enviroment with name %s cannot been initialized",
enviroment.name.c_str());
return nullptr;
}
m_active_envs.push_back(enviroment);
m_executor->add_node(
enviroment.env_ptr->getNode()->get_node_base_interface());
return enviroment.env_ptr;
}
EnvInterfaceSharedPtr EnvManager::loadEnv(const std::string &env_name,
const std::string &env_class_type) {
EnvInterfaceSharedPtr loaded_env = nullptr;
RCLCPP_INFO(this->get_logger(), "Loading enviroment '%s'", env_name.c_str());
try {
if (m_loader->isClassAvailable(env_class_type)) {
loaded_env = m_loader->createSharedInstance(env_class_type);
} else {
RCLCPP_ERROR(
this->get_logger(),
"Loading enviroment is not available '%s' with class type '%s'",
env_name.c_str(), env_class_type.c_str());
RCLCPP_INFO(this->get_logger(), "Available classes");
for (const auto &available_class : m_loader->getDeclaredClasses()) {
RCLCPP_INFO(this->get_logger(), "\t%s", available_class.c_str());
}
return nullptr;
}
} catch (const pluginlib::PluginlibException &e) {
RCLCPP_ERROR(
get_logger(),
"Error happened during creation of enviroment '%s' with type '%s':\n%s",
env_name.c_str(), env_class_type.c_str(), e.what());
return nullptr;
}
EnvSpec enviroment;
enviroment.name = env_name;
enviroment.type = env_class_type;
enviroment.env_ptr = loaded_env;
return addEnv(enviroment);
}
EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) {
auto it = std::find_if(
m_active_envs.begin(), m_active_envs.end(),
[&env_name](const EnvSpec &env) { return env.name == env_name; });
if (it != m_active_envs.end()) {
it->env_ptr->getNode()->deactivate();
it->env_ptr->getNode()->cleanup();
m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface());
m_active_envs.erase(it);
return EnvStateReturnType::SUCCESS;
} else {
RCLCPP_ERROR(this->get_logger(),
"Environment '%s' not found in the active environments list.",
env_name.c_str());
return EnvStateReturnType::ERROR;
}
return EnvStateReturnType::FAILURE;
}
// EnvInterfaceSharedPtr
// EnvManager::loadEnv(const std::string env_name)
// {
// std::string env_class_type = "some_default_namespace::" + env_name;
// return loadEnv(env_name, env_class_type);
// }
EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) {
RCLCPP_INFO(this->get_logger(), "Configuring enviroment '%s'", env_name.c_str());
auto it = std::find_if(
m_active_envs.begin(), m_active_envs.end(),
[&env_name](const EnvSpec &env) { return env.name == env_name; });
if (it == m_active_envs.end()) {
RCLCPP_ERROR(this->get_logger(),
"Environment '%s' not found in the active environments list.",
env_name.c_str());
return EnvStateReturnType::FAILURE;
}
it->env_ptr->configure();
it->env_ptr->getNode()->activate();
return EnvStateReturnType::SUCCESS;
}
} // namespace env_manager

21
env_manager/env_manager/src/run_env_manager.cpp Normal file
View file

@ -0,0 +1,21 @@
#include "rclcpp/rclcpp.hpp"
#include "env_manager/env_manager.hpp"
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<rclcpp::Executor> executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
std::string manager_node_name = "env_manager";
auto em = std::make_shared<env_manager::EnvManager>(executor, manager_node_name);
RCLCPP_INFO(em->get_logger(), "Env manager is starting");
em->loadEnv("gz_enviroment", "gz_enviroment::GzEnviroment");
em->configureEnv("gz_enviroment");
executor->add_node(em);
executor->spin();
rclcpp::shutdown();
return 0;
}

37
env_manager/env_manager_interfaces/CMakeLists.txt Normal file
View file

@ -0,0 +1,37 @@
cmake_minimum_required(VERSION 3.8)
project(env_manager_interfaces)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(lifecycle_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
srv/StartEnv.srv
srv/ConfigureEnv.srv
srv/LoadEnv.srv
srv/UnloadEnv.srv
msg/EnvState.msg
DEPENDENCIES lifecycle_msgs
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

202
env_manager/env_manager_interfaces/LICENSE Normal file
View file

@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

4
env_manager/env_manager_interfaces/msg/EnvState.msg Normal file
View file

@ -0,0 +1,4 @@
string name
string type
string plugin_name
lifecycle_msgs/State state

27
env_manager/env_manager_interfaces/package.xml Normal file
View file

@ -0,0 +1,27 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>env_manager_interfaces</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ur.narmak@gmail.com">solid-sinusoid</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<build_depend>builtin_interfaces</build_depend>
<build_depend>lifecycle_msgs</build_depend>
<exec_depend>builtin_interfaces</exec_depend>
<exec_depend>lifecycle_msgs</exec_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

3
env_manager/env_manager_interfaces/srv/ConfigureEnv.srv Normal file
View file

@ -0,0 +1,3 @@
string name
---
bool ok

4
env_manager/env_manager_interfaces/srv/LoadEnv.srv Normal file
View file

@ -0,0 +1,4 @@
string name
string type
---
bool ok

4
env_manager/env_manager_interfaces/srv/StartEnv.srv Normal file
View file

@ -0,0 +1,4 @@
string name
string type
---
bool ok

3
env_manager/env_manager_interfaces/srv/UnloadEnv.srv Normal file
View file

@ -0,0 +1,3 @@
string name
---
bool ok

72
env_manager/gz_enviroment/CMakeLists.txt Normal file
View file

@ -0,0 +1,72 @@
cmake_minimum_required(VERSION 3.8)
project(gz_enviroment)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
set(THIS_PACKAGE_INCLUDE_DEPENDS
rclcpp
rclcpp_lifecycle
tf2_ros
tf2_msgs
tf2_eigen
ros_gz
pluginlib
ignition-transport11
ignition-msgs8
ros_gz_bridge
env_interface
ignition-gazebo6
)
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()
# TODO: generate_parameter_library
add_library(${PROJECT_NAME} SHARED src/gz_enviroment.cpp)
target_include_directories(${PROJECT_NAME} PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/gz_enviroment>
)
ament_target_dependencies(${PROJECT_NAME} PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
target_compile_definitions(${PROJECT_NAME} PRIVATE "GZENVIROMENT_BUILDING_DLL")
#TODO: replace rclcpp_lifecycle with custom interface class
pluginlib_export_plugin_description_file(env_interface gz_enviroment.xml)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
install(
DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
install(TARGETS ${PROJECT_NAME}
EXPORT export_${PROJECT_NAME}
RUNTIME DESTINATION bin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
)
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
# ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
ament_package()

202
env_manager/gz_enviroment/LICENSE Normal file
View file

@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

7
env_manager/gz_enviroment/gz_enviroment.xml Normal file
View file

@ -0,0 +1,7 @@
<library path="gz_enviroment">
<class
type="gz_enviroment::GzEnviroment"
base_class_type="env_interface::EnvInterface">
<description>Gazebo enviroment for env_manager</description>
</class>
</library>

57
env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp Normal file
View file

@ -0,0 +1,57 @@
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
// #include "ros_gz_bridge/convert.hpp"
#include "env_interface/env_interface.hpp"
#include "gz/msgs/pose_v.pb.h"
#include <geometry_msgs/msg/transform_stamped.hpp>
#include "geometry_msgs/msg/pose_array.hpp"
#include <gz/transport/Node.hh>
#include <memory>
#include <mutex>
#include <string>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <gz/sim/Entity.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/components/Model.hh>
namespace gz_enviroment {
using CallbackReturn =
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
class GzEnviroment : public env_interface::EnvInterface {
public:
GzEnviroment();
CallbackReturn on_init() override;
CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
protected:
void onGzPoseSub(const gz::msgs::Pose_V &pose_v);
bool doGzSpawn();
private:
std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster;
std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster_target;
std::shared_ptr<gz::transport::Node> m_gz_node;
std::vector<std::string> m_follow_frames;
std::string m_topic_name;
std::string m_service_spawn;
std::string m_world_name;
std::shared_ptr<rbs_utils::AssemblyConfigLoader> m_config_loader;
tf2_msgs::msg::TFMessage m_transforms;
tf2_msgs::msg::TFMessage m_target_places;
std::string getGzWorldName();
std::string createGzModelString(const std::vector<double> &pose,
const std::vector<double> &inertia,
const double &mass,
const std::string &mesh_filepath,
const std::string &name);
};
} // namespace gz_enviroment

25
env_manager/gz_enviroment/package.xml Normal file
View file

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gz_enviroment</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>tf2_ros</depend>
<depend>tf2_msgs</depend>
<depend>ros_gz</depend>
<depend>env_interface</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

211
env_manager/gz_enviroment/src/gz_enviroment.cpp Normal file
View file

@ -0,0 +1,211 @@
#include "gz_enviroment/gz_enviroment.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "ros_gz_bridge/convert.hpp"
#include <memory>
#include <rclcpp/logging.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
#include <gz/sim/components.hh>
#include <string>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/transform_broadcaster.h>
namespace gz_enviroment {
GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {}
CallbackReturn GzEnviroment::on_init() {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()");
gz::sim::EntityComponentManager ecm;
const auto modelEntities =
ecm.EntitiesByComponents(gz::sim::components::Model());
for (const auto entity : modelEntities) {
const auto modelName = ecm.Component<gz::sim::components::Name>(entity);
if (modelName) {
RCLCPP_INFO_STREAM(getNode()->get_logger(),
"Model Name: " << modelName->Data());
}
}
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
// Configuration of parameters
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
m_gz_node = std::make_shared<gz::transport::Node>();
m_world_name = getGzWorldName();
m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
m_service_spawn = std::string("/world/") + m_world_name + "/create";
m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
"asp-example2", getNode());
m_follow_frames = m_config_loader->getSceneModelNames();
// m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>();
m_transforms = m_config_loader->getTfData("bishop");
// TODO add workspace viewer in Rviz
// m_config_loader->printWorkspace();
// if (!doGzSpawn())
// {
// return CallbackReturn::ERROR;
// }
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
// Setting up the subscribers and publishers
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");
m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
m_tf2_broadcaster =
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
m_tf2_broadcaster_target =
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup");
// Clear variables on_cleanup
m_gz_node.reset();
m_follow_frames.clear();
m_topic_name.clear();
m_service_spawn.clear();
m_world_name.clear();
m_config_loader.reset();
m_transforms = tf2_msgs::msg::TFMessage();
m_target_places = tf2_msgs::msg::TFMessage();
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate");
// What we should use here?
return CallbackReturn::SUCCESS;
}
// TODO: Check to do this via EntityComponentManager
void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
// TODO: Read from config
// m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"};
std::vector<geometry_msgs::msg::TransformStamped> all_transforms{};
for (const auto &it : pose_v.pose()) {
if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) ==
m_follow_frames.end())
continue;
geometry_msgs::msg::PoseStamped rpose;
ros_gz_bridge::convert_gz_to_ros(it, rpose);
geometry_msgs::msg::TransformStamped t;
t.header.stamp = getNode()->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = it.name();
t.transform.translation.x = rpose.pose.position.x;
t.transform.translation.y = rpose.pose.position.y;
t.transform.translation.z = rpose.pose.position.z;
t.transform.rotation.x = rpose.pose.orientation.x;
t.transform.rotation.y = rpose.pose.orientation.y;
t.transform.rotation.z = rpose.pose.orientation.z;
t.transform.rotation.w = rpose.pose.orientation.w;
all_transforms.push_back(t);
}
for (auto &place : m_transforms.transforms) {
all_transforms.push_back(place);
}
for (auto &transform : all_transforms) {
m_tf2_broadcaster->sendTransform(transform);
}
}
std::string GzEnviroment::getGzWorldName() {
bool executed{false};
bool result{false};
unsigned int timeout{5000};
std::string service{"/gazebo/worlds"};
gz::msgs::StringMsg_V worlds_msg;
while (rclcpp::ok() && !executed) {
RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names.");
executed = m_gz_node->Request(service, timeout, worlds_msg, result);
}
if (!executed) {
RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names.");
return "";
}
if (!result || worlds_msg.data().empty()) {
RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names.");
return "";
}
RCLCPP_INFO(getNode()->get_logger(), "%s", worlds_msg.data(0).c_str());
return worlds_msg.data(0);
}
bool GzEnviroment::doGzSpawn() {
gz::msgs::EntityFactory req;
gz::msgs::Boolean rep;
bool result;
unsigned int timeout = 5000;
bool executed;
auto env_models = m_config_loader->getEnvModels();
for (auto &model : *env_models) {
std::string sdf_string =
createGzModelString(model.model_pose, model.model_inertia, model.mass,
model.mesh_path, model.model_name);
req.set_sdf(sdf_string);
executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
}
return executed;
}
std::string GzEnviroment::createGzModelString(
const std::vector<double> &pose, const std::vector<double> &inertia,
const double &mass, const std::string &mesh_filepath,
const std::string &name) {
std::string model_template =
std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
"<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
"</pose>"
// + "<inertial>"
// + "<inertia>"
// + "<ixx>" + std::to_string(inertia[0]) + "</ixx>"
// + "<ixy>" + std::to_string(inertia[1]) + "</ixy>"
// + "<ixz>" + std::to_string(inertia[2]) + "</ixz>"
// + "<iyy>" + std::to_string(inertia[3]) + "</iyy>"
// + "<iyz>" + std::to_string(inertia[4]) + "</iyz>"
// + "<izz>" + std::to_string(inertia[5]) + "</izz>"
// + "</inertia>"
// + "<mass>" + std::to_string(mass) + "</mass>"
// + "</inertial>"
+ "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" +
mesh_filepath + "</uri></mesh></geometry>" + "</visual>" +
"<collision name='collision_" + name + "'>" +
"<geometry><mesh><uri>file://" + mesh_filepath +
"</uri></mesh></geometry>" + "</collision>" + "</link>" + "</model>" +
"</sdf>";
return model_template;
}
} // namespace gz_enviroment
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment,
env_interface::EnvInterface);

61
env_manager/include/component_manager/client_component.hpp
View file

@ -1,61 +0,0 @@
#ifndef ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_
#define ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_
#include "component_manager/visibility_control.h"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <cstdlib>
#include <future>
#include <memory>
namespace env_manager
{
namespace component_manager
{
using namespace std::chrono_literals;
const std::string DEFAULT_CLIENT_NODE_NAME = "env_manager_client_node";
const std::string DEFAULT_CLIENT_NAME = "client";
template <typename ServiceT>
class ClientComponent: public rclcpp::Node
{
public:
explicit ClientComponent(const rclcpp::NodeOptions& options)
: Node(DEFAULT_CLIENT_NODE_NAME, options)
{
_client = create_client<ServiceT>(DEFAULT_CLIENT_NAME, 10);
}
virtual void callback(
typename rclcpp::Client<ServiceT>::SharedFuture future) = 0;
void populate_request(
const std::shared_ptr<typename ServiceT::Request>& request)
{
while (!_client->wait_for_service(1s))
{
if (rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(),
"Client interrupted while waiting for service. Terminating...");
}
}
auto result_future = _client->async_send_request(
request, std::bind(&ClientComponent::callback,
this, std::placeholders::_1));
}
private:
typename rclcpp::Client<ServiceT>::SharedPtr _client;
};
} // namespace component_manager
} // namespace env_manager
#endif // ENV_MANAGER__COMPONENT_MANAGER__CLIENT_COMPONENT_HPP_

50
env_manager/include/component_manager/component_manager.hpp
View file

@ -1,50 +0,0 @@
#ifndef COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_
#define COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_
#include "component_manager/visibility_control.h"
#include "config/options.hpp"
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/component_manager.hpp>
#include <rclcpp_components/node_factory.hpp>
#include <class_loader/class_loader.hpp>
#include <glog/logging.h>
namespace env_manager
{
namespace component_manager
{
/**
* This class implements the system for managing and configuring loaded components.
*
* It is assumed that the loaded components are inherited from the classes
* provided in the library for each node type.
*/
class ComponentManager
{
public:
ComponentManager(std::weak_ptr<rclcpp::Executor> executor);
void register_components(
const std::map<std::string, config::ComponentOption> &comps,
const std::map<std::string, config::NodeOption> &nodes,
const std::string& ns);
void remove_components_from_executor();
void remap_components_namespace(const std::string& ns);
private:
std::weak_ptr<rclcpp::Executor> _executor;
std::vector<class_loader::ClassLoader* > _loaders;
std::map<std::string, rclcpp_components::NodeInstanceWrapper> _node_wrappers;
std::map<std::string, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr> _nodes;
};
} // namespace env_manager
} // namespace component_manager
#endif // COMPONENT_MANAGER__COMPONENT_MANAGER_HPP_

36
env_manager/include/component_manager/node_component.hpp
View file

@ -1,36 +0,0 @@
#ifndef ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__
#define ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__
#include "component_manager/visibility_control.h"
#include "rclcpp/node.hpp"
namespace env_manager
{
namespace component_manager
{
const std::string DEFAULT_NODE_NODE_NAME = "env_manager_node";
//TODO: interhit all another components from this node_component
class NodeComponent: public rclcpp::Node
{
public:
explicit NodeComponent(const rclcpp::NodeOptions& options)
:Node(DEFAULT_NODE_NODE_NAME, options)
{
auto ret = rcutils_logging_set_logger_level(
get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL);
if (ret != RCUTILS_RET_OK)
{
RCLCPP_ERROR(get_logger(),
"Error setting severity: %s", rcutils_get_error_string().str);
rcutils_reset_error();
}
}
};
} // namesapce component_manager
} // namesapce env_manager
#endif // ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__

46
env_manager/include/component_manager/publisher_component.hpp
View file

@ -1,46 +0,0 @@
#ifndef ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_
#define ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_
#include "component_manager/visibility_control.h"
#include "rclcpp/rclcpp.hpp"
namespace env_manager
{
namespace component_manager
{
const std::string DEFAULT_PUB_NODE_NAME = "env_manager_pub_node";
const std::string DEFAULT_PUB_TOPIC_NAME = "pub_topic";
template <typename MessageT>
class PublisherComponent: public rclcpp::Node
{
public:
explicit PublisherComponent(const rclcpp::NodeOptions& options)
: Node(DEFAULT_PUB_NODE_NAME, options)
{
_pub = create_publisher<MessageT>(DEFAULT_PUB_TOPIC_NAME, 10);
auto ret = rcutils_logging_set_logger_level(
get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL);
if (ret != RCUTILS_RET_OK)
{
RCLCPP_ERROR(get_logger(),
"Error setting severity: %s", rcutils_get_error_string().str);
rcutils_reset_error();
}
}
void populate_publication(const MessageT& msg)
{
_pub->publish(std::move(msg));
}
private:
typename rclcpp::Publisher<MessageT>::SharedPtr _pub;
};
} // namespace component_manager
} // namespace env_manager
#endif // ENV_MANAGER__COMPONENT_MANAGER__PUBLISHER_COMPONENT_HPP_

39
env_manager/include/component_manager/service_component.hpp
View file

@ -1,39 +0,0 @@
#ifndef ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_
#define ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_
#include "component_manager/visibility_control.h"
#include <rclcpp/node.hpp>
namespace env_manager
{
namespace component_manager
{
const std::string DEFAULT_SERVICE_NDOE_NAME = "env_manager_service_node";
const std::string DEFAULT_SERVICE_NAME = "service";
template <typename ServiceT>
class ServiceComponent: public rclcpp::Node
{
public:
explicit ServiceComponent(const rclcpp::NodeOptions& options)
: Node(DEFAULT_SERVICE_NDOE_NAME, options)
{
_service = create_service<ServiceT>(
DEFAULT_SERVICE_NAME, std::bind(
&ServiceComponent::callback, this,
std::placeholders::_1, std::placeholders::_2));
}
virtual void callback(std::shared_ptr<typename ServiceT::Request> request,
std::shared_ptr<typename ServiceT::Response> response) = 0;
private:
typename rclcpp::Service<ServiceT>::SharedPtr _service;
};
} // namespace component_manager
} // namespace env_manager
#endif // ENV_MANAGER__COMPONENT_MANAGER__SERVICE_COMPONENT_HPP_

44
env_manager/include/component_manager/subscriber_component.hpp
View file

@ -1,44 +0,0 @@
#ifndef ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_
#define ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_
#include <iostream>
#include <memory>
#include "rclcpp/rclcpp.hpp"
namespace env_manager
{
namespace component_manager
{
const std::string DEFAULT_SUB_NODE_NAME = "env_manager_sub_node";
const std::string DEFAULT_SUB_TOPIC_NAME = "sub_topic";
template <typename MessageT>
class SubscriberComponent : public rclcpp::Node
{
public:
explicit SubscriberComponent(const rclcpp::NodeOptions& options)
: Node(DEFAULT_SUB_NODE_NAME, options)
{
_sub = create_subscription<MessageT>(
DEFAULT_SUB_TOPIC_NAME, 10, this->callback);
auto ret = rcutils_logging_set_logger_level(
get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL);
if (ret != RCUTILS_RET_OK)
{
RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
rcutils_reset_error();
}
}
virtual void callback(const MessageT& msg) = 0;
private:
typename rclcpp::Subscription<MessageT>::SharedPtr _sub;
};
} // namespace component_manager
} // namespace env_manager
#endif // ENV_MANAGER_COMPONENT_MANAGER_SUBSCRIBER_COMPONENT_HPP_

35
env_manager/include/component_manager/visibility_control.h
View file

@ -1,35 +0,0 @@
#ifndef COMPONENT_MANAGER__VISIBILITY_CONTROL_H_
#define COMPONENT_MANAGER__VISIBILITY_CONTROL_H_
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define COMPONENT_MANAGER_EXPORT __attribute__ ((dllexport))
#define COMPONENT_MANAGER_IMPORT __attribute__ ((dllimport))
#else
#define COMPONENT_MANAGER_EXPORT __declspec(dllexport)
#define COMPONENT_MANAGER_IMPORT __declspec(dllimport)
#endif
#ifdef COMPONENT_MANAGER_BUILDING_LIBRARY
#define COMPONENT_MANAGER_PUBLIC COMPONENT_MANAGER_EXPORT
#else
#define COMPONENT_MANAGER_PUBLIC COMPONENT_MANAGER_IMPORT
#endif
#define COMPONENT_MANAGER_PUBLIC_TYPE COMPONENT_MANAGER_PUBLIC
#define COMPONENT_MANAGER_LOCAL
#else
#define COMPONENT_MANAGER_EXPORT __attribute__ ((visibility("default")))
#define COMPONENT_MANAGER_IMPORT
#if __GNUC__ >= 4
#define COMPONENT_MANAGER_PUBLIC __attribute__ ((visibility("default")))
#define COMPONENT_MANAGER_LOCAL __attribute__ ((visibility("hidden")))
#else
#define COMPONENT_MANAGER_PUBLIC
#define COMPONENT_MANAGER_LOCAL
#endif
#define COMPONENT_MANAGER_PUBLIC_TYPE
#endif
#endif // COMPONENT_MANAGER__VISIBILITY_CONTROL_H_

15
env_manager/include/config/config.hpp.cmake
View file

@ -1,15 +0,0 @@
#ifndef ENV_MANAGER_CONFIG_CONFIG_H_
#define ENV_MANAGER_CONFIG_CONFIG_H_
namespace env_manager
{
namespace config
{
constexpr char kConfigurationFilesDirectory[] = "@ENV_MANAGER_CONFIGURATION_FILES_DIRECTORY@";
constexpr char kSourceDirectory[] = "@PROJECT_SOURCE_DIR@";
} // namespace config
} // namespace env_manager
#endif // ENV_MANAGER_CONFIG_CONFIG_H_

28
env_manager/include/config/config_file_resolver.hpp
View file

@ -1,28 +0,0 @@
#ifndef ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_
#define ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_
#include "lua_file_resolver.hpp"
namespace env_manager
{
namespace config
{
class ConfigurationFileResolver : public FileResolver
{
public:
explicit ConfigurationFileResolver(
const std::vector<std::string>& cfg_files_dirs);
std::string GetPath(const std::string& basename) const;
std::string GetContent(const std::string& basename) const;
private:
std::vector<std::string> _config_files_dirs;
};
} // namespace config
} // namespace env_manager
#endif // ENV_MANAGER_CONFIG_CONFIG_FILE_RESOLVER_H_

129
env_manager/include/config/lua_file_resolver.hpp
View file

@ -1,129 +0,0 @@
#ifndef ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_
#define ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_
#include "lua.hpp"
#include <map>
#include <memory>
#include <string>
#include <vector>
namespace env_manager
{
namespace config
{
class FileResolver
{
public:
virtual ~FileResolver() {}
virtual std::string GetPath(const std::string& basename) const = 0;
virtual std::string GetContent(const std::string& basename) const = 0;
};
class LuaParameterDictionary {
public:
// Constructs the dictionary from a Lua Table specification.
LuaParameterDictionary(const std::string& code,
std::unique_ptr<FileResolver> file_resolver);
LuaParameterDictionary(const LuaParameterDictionary&) = delete;
LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete;
// Constructs a LuaParameterDictionary without reference counting.
static std::unique_ptr<LuaParameterDictionary> NonReferenceCounted(
const std::string& code, std::unique_ptr<FileResolver> file_resolver);
~LuaParameterDictionary();
// Returns all available keys.
std::vector<std::string> GetKeys() const;
// Returns true if the key is in this dictionary.
bool HasKey(const std::string& key) const;
// These methods CHECK() that the 'key' exists.
std::string GetString(const std::string& key);
double GetDouble(const std::string& key);
int GetInt(const std::string& key);
bool GetBool(const std::string& key);
std::unique_ptr<LuaParameterDictionary> GetDictionary(const std::string& key);
// Gets an int from the dictionary and CHECK()s that it is non-negative.
int GetNonNegativeInt(const std::string& key);
// Returns a string representation for this LuaParameterDictionary.
std::string ToString() const;
// Returns the values of the keys '1', '2', '3' as the given types.
std::vector<double> GetArrayValuesAsDoubles();
std::vector<std::string> GetArrayValuesAsStrings();
std::vector<std::unique_ptr<LuaParameterDictionary>>
GetArrayValuesAsDictionaries();
private:
enum class ReferenceCount { YES, NO };
LuaParameterDictionary(const std::string& code,
ReferenceCount reference_count,
std::unique_ptr<FileResolver> file_resolver);
// For GetDictionary().
LuaParameterDictionary(lua_State* L, ReferenceCount reference_count,
std::shared_ptr<FileResolver> file_resolver);
// Function that recurses to keep track of indent for ToString().
std::string DoToString(const std::string& indent) const;
// Pop the top of the stack and CHECKs that the type is correct.
double PopDouble() const;
int PopInt() const;
bool PopBool() const;
// Pop the top of the stack and CHECKs that it is a string. The returned value
// is either quoted to be suitable to be read back by a Lua interpretor or
// not.
enum class Quoted { YES, NO };
std::string PopString(Quoted quoted) const;
// Creates a LuaParameterDictionary from the Lua table at the top of the
// stack, either with or without reference counting.
std::unique_ptr<LuaParameterDictionary> PopDictionary(
ReferenceCount reference_count) const;
// CHECK() that 'key' is in the dictionary.
void CheckHasKey(const std::string& key) const;
// CHECK() that 'key' is in this dictionary and reference it as being used.
void CheckHasKeyAndReference(const std::string& key);
// If desired, this can be called in the destructor of a derived class. It
// will CHECK() that all keys defined in the configuration have been used
// exactly once and resets the reference counter.
void CheckAllKeysWereUsedExactlyOnceAndReset();
// Reads a file into a Lua string.
static int LuaRead(lua_State* L);
// Handles inclusion of other Lua files and prevents double inclusion.
static int LuaInclude(lua_State* L);
lua_State* L_; // The name is by convention in the Lua World.
int index_into_reference_table_;
// This is shared with all the sub dictionaries.
const std::shared_ptr<FileResolver> file_resolver_;
// If true will check that all keys were used on destruction.
const ReferenceCount reference_count_;
// This is modified with every call to Get* in order to verify that all
// parameters are read exactly once.
std::map<std::string, int> reference_counts_;
// List of all included files in order of inclusion. Used to prevent double
// inclusion.
std::vector<std::string> included_files_;
};
} // namespce config
} // namespace env_manager
#endif // ENV_MANAGER_CONFIG_LUA_FILE_RESOLVER_H_

58
env_manager/include/config/options.hpp
View file

@ -1,58 +0,0 @@
#ifndef ENV_MANAGER_CONFIG_OPTIONS_HPP_
#define ENV_MANAGER_CONFIG_OPTIONS_HPP_
#include "config/lua_file_resolver.hpp"
#include <unordered_map>
namespace env_manager
{
namespace config
{
struct ComponentOption
{
static ComponentOption create_option(
LuaParameterDictionary* lua_parameter_dictionary);
std::string library;
std::string class_name;
};
struct EnvironmentOption
{
static EnvironmentOption create_option(
LuaParameterDictionary* lua_parameter_dictionary);
std::string ns;
std::map<std::string, ComponentOption> components;
};
struct NodeOption
{
enum NodeType
{
Publisher,
Subscriber,
Service,
Client,
Node
};
static NodeOption create_option(
LuaParameterDictionary* lua_parameter_dictionary);
std::string name;
std::string msg_type;
NodeType type;
static const std::unordered_map<std::string, NodeType> node_type_remap;
};
struct EnvManagerOption
{
static EnvManagerOption create_option(
LuaParameterDictionary* lua_parameter_dictionary);
std::map<std::string, EnvironmentOption> environments;
std::map<std::string, NodeOption> nodes;
};
}
}
#endif // ENV_MANAGER_CONFIG_OPTIONS_HPP_

0
env_manager/include/env_manager/environment_base.hpp
View file

0
env_manager/include/env_manager/environment_manager.hpp
View file

30
env_manager/include/env_manager/environment_node_manager.hpp
View file

@ -1,30 +0,0 @@
#ifndef ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_
#define ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_
#include "config/options.hpp"
#include <vector>
#include <rclcpp/executors.hpp>
#include <boost/thread.hpp>
extern "C"
{
#include <rcl/context.h>
#include <rcl/node.h>
}
namespace env_manager
{
class EnvironmentNodeManager
{
public:
EnvironmentNodeManager();
~EnvironmentNodeManager();
};
} // namespace env_manager
#endif // ENV_MANAGER__ENVIRONMENT_NODE_MANAGER_HPP_

26
env_manager/planning_scene_manager/CMakeLists.txt Normal file
View file

@ -0,0 +1,26 @@
cmake_minimum_required(VERSION 3.8)
project(planning_scene_manager)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

202
env_manager/planning_scene_manager/LICENSE Normal file
View file

@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

29
env_manager/planning_scene_manager/include/planning_scene_manager/planning_scene_manager.hpp Normal file
View file

@ -0,0 +1,29 @@
#ifndef __PLANNING_SCENE_MANAGER_H__
#define __PLANNING_SCENE_MANAGER_H__
#include "rclcpp/rclcpp.hpp"
#include "moveit/planning_scene_monitor/planning_scene_monitor.h"
namespace planning_scene_manager
{
class PlanningSceneManager : public planning_scene_monitor::PlanningSceneMonitor
{
public:
PlanningSceneManager();
bool GetPlanningObject();
bool SetPlanningObject();
bool SetPlanningScene();
bool GetPlanningScene();
bool UpdatePlanningScene();
private:
};
} // namespace planning_scene_manager
#endif // __PLANNING_SCENE_MANAGER_H__

13
env_manager/package.xml → env_manager/planning_scene_manager/package.xml
View file

@ -1,16 +1,13 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>env_manager</name>
<name>planning_scene_manager</name>
<version>0.0.0</version>
<description>env_manager</description>
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
<license>Apache-2.0</license>
<description>TODO: Package description</description>
<maintainer email="ur.narmak@gmail.com">Bill Finger</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake_ros</buildtool_depend>
<depend>boost</depend>
<depend>rcl</depend>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

31
env_manager/planning_scene_manager/src/planning_scene_manager.cpp Normal file
View file

@ -0,0 +1,31 @@
#include "planning_scene_manager/planning_scene_manager.hpp"
#include "moveit_msgs/msg/collision_object.hpp"
#include "moveit/planning_scene_interface/planning_scene_interface.h"
namespace planning_scene_manager
{
bool PlanningSceneManager::SetPlanningObject()
{
moveit_msgs::msg::CollisionObject object;
std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> pcm_;
planning_scene_monitor::LockedPlanningSceneRW scene(pcm_);
scene->removeAllCollisionObjects();
RCLCPP_INFO(pnode_->get_logger(), "All object has been delete");
return true;
}
} // namespace planning_scene_manager
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<planning_scene_manager::PlanningSceneManager>();
// Добавляем объект в сцену коллизии
node->SetPlanningObject();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

159
env_manager/src/component_manager/component_manager.cpp
View file

@ -1,159 +0,0 @@
#include "component_manager/component_manager.hpp"
#include "component_manager/node_component.hpp"
#include "component_manager/publisher_component.hpp"
#include "component_manager/subscriber_component.hpp"
#include "component_manager/service_component.hpp"
#include "component_manager/client_component.hpp"
#include "glog/logging.h"
#include "rcl/remap.h"
#include <iostream>
namespace env_manager
{
namespace component_manager
{
ComponentManager::ComponentManager(std::weak_ptr<rclcpp::Executor> executor)
: _executor(executor)
{
}
static rclcpp::NodeOptions create_options(
const config::NodeOption &node_opts,
const std::string &node_name,
const std::string &ns)
{
rclcpp::NodeOptions opts;
std::vector<std::string> args = {"--ros-args",
"--disable-rosout-logs",
"--disable-stdout-logs",
"--enable-external-lib-logs",
"--log-level", "WARN",
};
args.push_back("-r");
args.push_back("__ns:=/" + ns);
args.push_back("-r");
args.push_back("__node:=" + node_name);
args.push_back("-r");
switch (node_opts.type)
{
case config::NodeOption::NodeType::Publisher:
args.push_back(DEFAULT_PUB_TOPIC_NAME + ":=" + node_opts.name);
break;
case config::NodeOption::NodeType::Subscriber:
args.push_back(DEFAULT_SUB_TOPIC_NAME + ":=" + node_opts.name);
break;
case config::NodeOption::NodeType::Service:
args.push_back(DEFAULT_SERVICE_NAME + ":=" + node_opts.name);
break;
case config::NodeOption::NodeType::Client:
args.push_back(DEFAULT_CLIENT_NAME + ":=" + node_opts.name);
break;
case config::NodeOption::NodeType::Node:
args.erase(args.end());
break;
}
opts.arguments(args);
return opts;
}
static rclcpp::NodeOptions create_default_options(const std::string &ns)
{
rclcpp::NodeOptions opts;
opts.arguments({
"--ros-args", "-r", std::string("__ns:=/" + ns),
"--disable-rosout-logs",
"--disable-stdout-logs",
"--enable-external-lib-logs",
"--log-level", "FATAL",
});
return opts;
}
void ComponentManager::register_components(
const std::map<std::string, config::ComponentOption> &comps,
const std::map<std::string, config::NodeOption> &nodes,
const std::string& ns)
{
if (comps.empty())
return;
for (const auto &[name, comp]: comps)
{
auto class_name = "rclcpp_components::NodeFactoryTemplate<" + comp.class_name + ">";
LOG(INFO) << "Provide lib: " << comp.library << " namespace: " + ns;
auto loader = new class_loader::ClassLoader(comp.library);
auto classes =
loader->getAvailableClasses<rclcpp_components::NodeFactory>();
for (auto clazz: classes)
{
if (clazz != class_name)
continue;
rclcpp::NodeOptions opts = create_default_options(ns);
if (clazz == class_name)
{
auto node_opts = nodes.at(name);
opts = create_options(node_opts, name, ns);
}
LOG(INFO) << "Create instance of class: " << clazz;
auto node_factory =
loader->createInstance<rclcpp_components::NodeFactory>(clazz);
auto wrapper = node_factory->create_node_instance(opts);
auto node = wrapper.get_node_base_interface();
_node_wrappers.insert({name, wrapper});
_nodes.insert({name, node});
if (auto exec = _executor.lock())
exec->add_node(node);
}
_loaders.push_back(loader);
}
}
void ComponentManager::remap_components_namespace(const std::string &ns)
{
char* nms = const_cast<char*>(ns.c_str());
for (auto& [name, wrapper]: _node_wrappers)
{
auto node = (rclcpp::Node*)wrapper.get_node_instance().get();
auto opts = node->get_node_options();
auto ret = rcl_remap_node_namespace(
&opts.get_rcl_node_options()->arguments,
NULL, node->get_name(), rcl_get_default_allocator(),
&nms);
if (ret == RCL_RET_OK)
LOG(INFO) << "Succesfully remap node with ns: " + ns;
}
//std::logic_error("Not implemented." + ns);
}
void ComponentManager::remove_components_from_executor()
{
if (_nodes.empty())
{
LOG(WARNING) << "Unable to remove nodes from executor.";
return;
}
if (auto exec = _executor.lock())
{
for (const auto &[node_name, node]: _nodes)
{
LOG(INFO) << "Remove node '" << node_name << "' from executor.";
exec->remove_node(node);
}
}
}
} // namespace env_manager
} // namespace component_manager

51
env_manager/src/config/config_file_resolver.cpp
View file

@ -1,51 +0,0 @@
#include "config/config_file_resolver.hpp"
#include <fstream>
#include <iostream>
#include <streambuf>
#include "config/config.hpp"
#include <glog/logging.h>
namespace env_manager
{
namespace config
{
ConfigurationFileResolver::ConfigurationFileResolver(
const std::vector<std::string>& cfg_files_dirs)
: _config_files_dirs(cfg_files_dirs)
{
_config_files_dirs.push_back(kConfigurationFilesDirectory);
}
std::string ConfigurationFileResolver::GetPath(
const std::string &basename) const
{
for (const auto& path: _config_files_dirs)
{
const std::string filename = path + "/" + basename;
std::ifstream stream(filename.c_str());
if (stream.good())
{
LOG(INFO) << "found filename: " << filename;
return filename;
}
}
LOG(FATAL) << "File '" << basename << "' was not found.";
}
std::string ConfigurationFileResolver::GetContent(
const std::string &basename) const
{
CHECK(!basename.empty()) << "File basename cannot be empty." << basename;
const std::string filename = GetPath(basename);
std::ifstream stream(filename.c_str());
return std::string(std::istreambuf_iterator<char>(stream),
std::istreambuf_iterator<char>());
}
} // namespace config
} // namespace env_manager

446
env_manager/src/config/lua_file_resolver.cpp
View file

@ -1,446 +0,0 @@
#include "config/lua_file_resolver.hpp"
#include "glog/logging.h"
#include <algorithm>
#include <cmath>
#include <functional>
#include <memory>
namespace env_manager
{
namespace config
{
namespace {
// Replace the string at the top of the stack through a quoted version that Lua
// can read back.
void QuoteStringOnStack(lua_State* L) {
CHECK(lua_isstring(L, -1)) << "Top of stack is not a string value.";
int current_index = lua_gettop(L);
// S: ... string
lua_pushglobaltable(L); // S: ... string globals
lua_getfield(L, -1, "string"); // S: ... string globals <string module>
lua_getfield(L, -1,
"format"); // S: ... string globals <string module> format
lua_pushstring(L, "%q"); // S: ... string globals <string module> format "%q"
lua_pushvalue(L, current_index); // S: ... string globals <string module>
// format "%q" string
lua_call(L, 2, 1); // S: ... string globals <string module> quoted
lua_replace(L, current_index); // S: ... quoted globals <string module>
lua_pop(L, 2); // S: ... quoted
}
// Sets the given 'dictionary' as the value of the "this" key in Lua's registry
// table.
void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* dictionary) {
lua_pushstring(L, "this");
lua_pushlightuserdata(L, dictionary);
lua_settable(L, LUA_REGISTRYINDEX);
}
// Gets the 'dictionary' from the "this" key in Lua's registry table.
LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) {
lua_pushstring(L, "this");
lua_gettable(L, LUA_REGISTRYINDEX);
void* return_value = lua_isnil(L, -1) ? nullptr : lua_touserdata(L, -1);
lua_pop(L, 1);
CHECK(return_value != nullptr);
return reinterpret_cast<LuaParameterDictionary*>(return_value);
}
// CHECK()s if a Lua method returned an error.
void CheckForLuaErrors(lua_State* L, int status) {
CHECK_EQ(status, 0) << lua_tostring(L, -1);
}
// Returns 'a' if 'condition' is true, else 'b'.
int LuaChoose(lua_State* L) {
CHECK_EQ(lua_gettop(L), 3) << "choose() takes (condition, a, b).";
CHECK(lua_isboolean(L, 1)) << "condition is not a boolean value.";
const bool condition = lua_toboolean(L, 1);
if (condition) {
lua_pushvalue(L, 2);
} else {
lua_pushvalue(L, 3);
}
return 1;
}
// Pushes a value to the Lua stack.
void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); }
void PushValue(lua_State* L, const std::string& key) {
lua_pushstring(L, key.c_str());
}
// Reads the value with the given 'key' from the Lua dictionary and pushes it to
// the top of the stack.
template <typename T>
void GetValueFromLuaTable(lua_State* L, const T& key) {
PushValue(L, key);
lua_rawget(L, -2);
}
// CHECK() that the topmost parameter on the Lua stack is a table.
void CheckTableIsAtTopOfStack(lua_State* L) {
CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
}
// Returns true if 'key' is in the table at the top of the Lua stack.
template <typename T>
bool HasKeyOfType(lua_State* L, const T& key) {
CheckTableIsAtTopOfStack(L);
PushValue(L, key);
lua_rawget(L, -2);
const bool key_not_found = lua_isnil(L, -1);
lua_pop(L, 1); // Pop the item again.
return !key_not_found;
}
// Iterates over the integer keys of the table at the top of the stack of 'L•
// and pushes the values one by one. 'pop_value' is expected to pop a value and
// put them into a C++ container.
void GetArrayValues(lua_State* L, const std::function<void()>& pop_value) {
int idx = 1;
while (true) {
GetValueFromLuaTable(L, idx);
if (lua_isnil(L, -1)) {
lua_pop(L, 1);
break;
}
pop_value();
++idx;
}
}
} // namespace
std::unique_ptr<LuaParameterDictionary>
LuaParameterDictionary::NonReferenceCounted(
const std::string& code, std::unique_ptr<FileResolver> file_resolver) {
return std::unique_ptr<LuaParameterDictionary>(new LuaParameterDictionary(
code, ReferenceCount::NO, std::move(file_resolver)));
}
LuaParameterDictionary::LuaParameterDictionary(
const std::string& code, std::unique_ptr<FileResolver> file_resolver)
: LuaParameterDictionary(code, ReferenceCount::YES,
std::move(file_resolver)) {}
LuaParameterDictionary::LuaParameterDictionary(
const std::string& code, ReferenceCount reference_count,
std::unique_ptr<FileResolver> file_resolver)
: L_(luaL_newstate()),
index_into_reference_table_(-1),
file_resolver_(std::move(file_resolver)),
reference_count_(reference_count) {
CHECK_NOTNULL(L_);
SetDictionaryInRegistry(L_, this);
luaL_openlibs(L_);
lua_register(L_, "choose", LuaChoose);
lua_register(L_, "include", LuaInclude);
lua_register(L_, "read", LuaRead);
CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str()));
CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0));
CheckTableIsAtTopOfStack(L_);
}
LuaParameterDictionary::LuaParameterDictionary(
lua_State* const L, ReferenceCount reference_count,
std::shared_ptr<FileResolver> file_resolver)
: L_(lua_newthread(L)),
file_resolver_(std::move(file_resolver)),
reference_count_(reference_count) {
CHECK_NOTNULL(L_);
// Make sure this is never garbage collected.
CHECK(lua_isthread(L, -1));
index_into_reference_table_ = luaL_ref(L, LUA_REGISTRYINDEX);
CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!";
lua_xmove(L, L_, 1); // Moves the table and the coroutine over.
CheckTableIsAtTopOfStack(L_);
}
LuaParameterDictionary::~LuaParameterDictionary() {
if (reference_count_ == ReferenceCount::YES) {
//CheckAllKeysWereUsedExactlyOnceAndReset();
}
if (index_into_reference_table_ > 0) {
luaL_unref(L_, LUA_REGISTRYINDEX, index_into_reference_table_);
} else {
lua_close(L_);
}
}
std::vector<std::string> LuaParameterDictionary::GetKeys() const {
CheckTableIsAtTopOfStack(L_);
std::vector<std::string> keys;
lua_pushnil(L_); // Push the first key
while (lua_next(L_, -2) != 0) {
lua_pop(L_, 1); // Pop value, keep key.
if (!lua_isnumber(L_, -1)) {
keys.emplace_back(lua_tostring(L_, -1));
}
}
return keys;
}
bool LuaParameterDictionary::HasKey(const std::string& key) const {
return HasKeyOfType(L_, key);
}
std::string LuaParameterDictionary::GetString(const std::string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopString(Quoted::NO);
}
std::string LuaParameterDictionary::PopString(Quoted quoted) const {
CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value.";
if (quoted == Quoted::YES) {
QuoteStringOnStack(L_);
}
const std::string value = lua_tostring(L_, -1);
lua_pop(L_, 1);
return value;
}
double LuaParameterDictionary::GetDouble(const std::string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopDouble();
}
double LuaParameterDictionary::PopDouble() const {
CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
const double value = lua_tonumber(L_, -1);
lua_pop(L_, 1);
return value;
}
int LuaParameterDictionary::GetInt(const std::string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopInt();
}
int LuaParameterDictionary::PopInt() const {
CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value.";
const int value = lua_tointeger(L_, -1);
lua_pop(L_, 1);
return value;
}
bool LuaParameterDictionary::GetBool(const std::string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopBool();
}
bool LuaParameterDictionary::PopBool() const {
CHECK(lua_isboolean(L_, -1)) << "Top of stack is not a boolean value.";
const bool value = lua_toboolean(L_, -1);
lua_pop(L_, 1);
return value;
}
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::GetDictionary(
const std::string& key) {
CheckHasKeyAndReference(key);
GetValueFromLuaTable(L_, key);
return PopDictionary(reference_count_);
}
std::unique_ptr<LuaParameterDictionary> LuaParameterDictionary::PopDictionary(
ReferenceCount reference_count) const {
CheckTableIsAtTopOfStack(L_);
std::unique_ptr<LuaParameterDictionary> value(
new LuaParameterDictionary(L_, reference_count, file_resolver_));
// The constructor lua_xmove()s the value, no need to pop it.
CheckTableIsAtTopOfStack(L_);
return value;
}
std::string LuaParameterDictionary::DoToString(
const std::string& indent) const {
std::string result = "{";
bool dictionary_is_empty = true;
const auto top_of_stack_to_string = [this, indent,
&dictionary_is_empty]() -> std::string {
dictionary_is_empty = false;
const int value_type = lua_type(L_, -1);
switch (value_type) {
case LUA_TBOOLEAN:
return PopBool() ? "true" : "false";
break;
case LUA_TSTRING:
return PopString(Quoted::YES);
break;
case LUA_TNUMBER: {
const double value = PopDouble();
if (std::isinf(value)) {
return value < 0 ? "-math.huge" : "math.huge";
} else {
return std::to_string(value);
}
} break;
case LUA_TTABLE: {
std::unique_ptr<LuaParameterDictionary> subdict(
PopDictionary(ReferenceCount::NO));
return subdict->DoToString(indent + " ");
} break;
default:
LOG(FATAL) << "Unhandled type " << lua_typename(L_, value_type);
}
};
// Integer (array) keys.
for (int i = 1; i; ++i) {
GetValueFromLuaTable(L_, i);
if (lua_isnil(L_, -1)) {
lua_pop(L_, 1);
break;
}
result.append("\n");
result.append(indent);
result.append(" ");
result.append(top_of_stack_to_string());
result.append(",");
}
// String keys.
std::vector<std::string> keys = GetKeys();
if (!keys.empty()) {
std::sort(keys.begin(), keys.end());
for (const std::string& key : keys) {
GetValueFromLuaTable(L_, key);
result.append("\n");
result.append(indent);
result.append(" ");
result.append(key);
result.append(" = ");
result.append(top_of_stack_to_string());
result.append(",");
}
}
result.append("\n");
result.append(indent);
result.append("}");
if (dictionary_is_empty) {
return "{}";
}
return result;
}
std::string LuaParameterDictionary::ToString() const { return DoToString(""); }
std::vector<double> LuaParameterDictionary::GetArrayValuesAsDoubles() {
std::vector<double> values;
GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); });
return values;
}
std::vector<std::unique_ptr<LuaParameterDictionary>>
LuaParameterDictionary::GetArrayValuesAsDictionaries() {
std::vector<std::unique_ptr<LuaParameterDictionary>> values;
GetArrayValues(L_, [&values, this] {
values.push_back(PopDictionary(reference_count_));
});
return values;
}
std::vector<std::string> LuaParameterDictionary::GetArrayValuesAsStrings() {
std::vector<std::string> values;
GetArrayValues(L_,
[&values, this] { values.push_back(PopString(Quoted::NO)); });
return values;
}
void LuaParameterDictionary::CheckHasKey(const std::string& key) const {
CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n"
<< ToString();
}
void LuaParameterDictionary::CheckHasKeyAndReference(const std::string& key) {
CheckHasKey(key);
reference_counts_[key]++;
}
void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() {
for (const auto& key : GetKeys()) {
CHECK_EQ(1, reference_counts_.count(key))
<< "Key '" << key << "' was used the wrong number of times.";
CHECK_EQ(1, reference_counts_.at(key))
<< "Key '" << key << "' was used the wrong number of times.";
}
reference_counts_.clear();
}
int LuaParameterDictionary::GetNonNegativeInt(const std::string& key) {
const int temp = GetInt(key); // Will increase reference count.
CHECK_GE(temp, 0) << temp << " is negative.";
return temp;
}
// Lua function to run a script in the current Lua context. Takes the filename
// as its only argument.
int LuaParameterDictionary::LuaInclude(lua_State* L) {
CHECK_EQ(lua_gettop(L), 1);
CHECK(lua_isstring(L, -1)) << "include takes a filename.";
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
const std::string basename = lua_tostring(L, -1);
const std::string filename =
parameter_dictionary->file_resolver_->GetPath(basename);
if (std::find(parameter_dictionary->included_files_.begin(),
parameter_dictionary->included_files_.end(),
filename) != parameter_dictionary->included_files_.end()) {
std::string error_msg =
"Tried to include " + filename +
" twice. Already included files in order of inclusion: ";
for (const std::string& filename : parameter_dictionary->included_files_) {
error_msg.append(filename);
error_msg.append("\n");
}
LOG(FATAL) << error_msg;
}
parameter_dictionary->included_files_.push_back(filename);
lua_pop(L, 1);
CHECK_EQ(lua_gettop(L), 0);
const std::string content =
parameter_dictionary->file_resolver_->GetContent(basename);
CheckForLuaErrors(
L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str()));
CheckForLuaErrors(L, lua_pcall(L, 0, LUA_MULTRET, 0));
return lua_gettop(L);
}
// Lua function to read a file into a string.
int LuaParameterDictionary::LuaRead(lua_State* L) {
CHECK_EQ(lua_gettop(L), 1);
CHECK(lua_isstring(L, -1)) << "read takes a filename.";
LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L);
const std::string file_content =
parameter_dictionary->file_resolver_->GetContent(
lua_tostring(L, -1));
lua_pushstring(L, file_content.c_str());
return 1;
}
} // namespace config
} // namespace env_manager

83
env_manager/src/config/options.cpp
View file

@ -1,83 +0,0 @@
#include "config/options.hpp"
#include "glog/logging.h"
namespace env_manager
{
namespace config
{
ComponentOption ComponentOption::create_option(
LuaParameterDictionary *lua_parameter_dictionary)
{
return ComponentOption{
lua_parameter_dictionary->GetString("lib"),
lua_parameter_dictionary->GetString("class"),
};
}
EnvironmentOption EnvironmentOption::create_option(
LuaParameterDictionary *lua_parameter_dictionary)
{
auto comps =
lua_parameter_dictionary->GetDictionary("components");
std::map<std::string, ComponentOption> components;
for (const auto &comp: comps->GetKeys())
components.insert({
comp, ComponentOption::create_option(comps->GetDictionary(comp).get())
});
return EnvironmentOption{
lua_parameter_dictionary->GetString("namespace"),
components,
};
}
NodeOption NodeOption::create_option(
LuaParameterDictionary *lua_parameter_dictionary)
{
return NodeOption{
lua_parameter_dictionary->GetString("name"),
lua_parameter_dictionary->GetString("msg_type"),
node_type_remap.at(lua_parameter_dictionary->GetString("type")),
};
}
const std::unordered_map<std::string, NodeOption::NodeType>
NodeOption::node_type_remap =
{
{"Publisher", NodeOption::NodeType::Publisher},
{"Subscriber", NodeOption::NodeType::Subscriber},
{"Service", NodeOption::NodeType::Service},
{"Client", NodeOption::NodeType::Client},
{"Node", NodeOption::NodeType::Node},
};
EnvManagerOption EnvManagerOption::create_option(
LuaParameterDictionary *lua_parameter_dictionary)
{
auto lua_env_dictionary = lua_parameter_dictionary->GetDictionary("environments");
auto envs_dict = lua_env_dictionary->GetArrayValuesAsDictionaries();
std::map<std::string, EnvironmentOption> environments;
for (const auto& env: envs_dict)
environments.insert({
env->GetString("namespace"), EnvironmentOption::create_option(env.get())
});
auto lua_node_dictionary = lua_parameter_dictionary->GetDictionary("nodes");
auto nodes_keys = lua_node_dictionary->GetKeys();
std::map<std::string, NodeOption> nodes;
for (const auto& node: nodes_keys)
nodes.insert({
node, NodeOption::create_option(lua_node_dictionary->GetDictionary(node).get())
});
return EnvManagerOption{
environments,
nodes,
};
}
} // namespace config
} // namespace env_manager

6
env_manager/src/env_manager/environment_node_manager.cpp
View file

@ -1,6 +0,0 @@
#include "env_manager/environment_node_manager.hpp"
namespace env_manager
{
} // namespace env_manager

60
env_manager/src/main.cpp
View file

@ -1,60 +0,0 @@
#include "component_manager/component_manager.hpp"
#include "config/config_file_resolver.hpp"
#include "config/lua_file_resolver.hpp"
#include "config/options.hpp"
#include "config/config.hpp"
#include "glog/logging.h"
int main(int argc, const char* argv[])
{
google::InitGoogleLogging(argv[0]);
FLAGS_logtostderr = 1;
rclcpp::init(argc, argv);
auto exec = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
std::vector<std::string> dirs = {};
auto cfg_resolver =
::std::make_unique<::env_manager::config::ConfigurationFileResolver>(
std::vector<std::string>{
std::string(env_manager::config::kSourceDirectory) +
"/config"
}
);
auto code = cfg_resolver->GetContent("config.lua");
//LOG(INFO) << code;
::env_manager::config::LuaParameterDictionary lua_dict(
code, std::move(cfg_resolver)
);
auto env_manager_opts =
::env_manager::config::EnvManagerOption::create_option(
&lua_dict
);
std::vector<env_manager::component_manager::ComponentManager> comp_mngrs;
for (const auto& [env, opts]: env_manager_opts.environments)
{
LOG(INFO) << "Start to initialize environment: " << env;
env_manager::component_manager::ComponentManager cmg(exec);
cmg.register_components(opts.components, env_manager_opts.nodes, opts.ns);
//cmg.upload_components_to_executor(&exec);
comp_mngrs.push_back(std::move(cmg));
}
//std::string ns = "/";
//comp_mngrs.begin()->remap_components_namespace(ns);
exec->spin();
for (auto &cmg: comp_mngrs)
cmg.remove_components_from_executor();
rclcpp::shutdown();
return 0;
}

21
rbs.real.repos Normal file
View file

@ -0,0 +1,21 @@
repositories:
ur_description:
type: git
url: https://github.com/solid-sinusoid/Universal_Robots_ROS2_Description.git
version: cartesian-controllers
ur_driver:
type: git
url: https://github.com/solid-sinusoid/Universal_Robots_ROS2_Driver.git
version: configure-with-gripper
behavior_tree:
type: git
url: https://github.com/solid-sinusoid/behavior_tree.git
version: master
rbs_schunk_40_n_n_b:
type: git
url: https://gitlab.com/robossembler/arm-tools/urdf-model-shrunk-gripper-egp-40-n-n-b.git
version: 2-add-ros2-control
gz_ros2_control:
type: git
url: https://github.com/solid-sinusoid/gz_ros2_control.git
version: ft-sensor-broadcaster

12
rbs.repos → rbs.sim.repos
View file

@ -11,15 +11,11 @@ repositories:
type: git
url: https://github.com/solid-sinusoid/behavior_tree.git
version: master
ros2_robotiq_gripper:
rbs_schunk_40_n_n_b:
type: git
url: https://github.com/solid-sinusoid/ros2_robotiq_gripper.git
version: dev
# cartesian_controllers:
# type: git
# url: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers.git
# version: ros2
url: https://gitlab.com/robossembler/arm-tools/urdf-model-shrunk-gripper-egp-40-n-n-b.git
version: 2-add-ros2-control
gz_ros2_control:
type: git
url: https://github.com/solid-sinusoid/gz_ros2_control.git
version: ft-sensor-broadcaster
version: ft-sensor-broadcaster

6
rbs_bringup/CMakeLists.txt
View file

@ -12,10 +12,14 @@ find_package(ament_cmake REQUIRED)
# find_package(<dependency> REQUIRED)
install(
DIRECTORY launch
DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
)
# Install Python modules
ament_python_install_package(rbs_launch_utils)
ament_python_install_module(rbs_launch_utils/launch_common.py)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights

622
rbs_bringup/config/fork_view.rviz Normal file
View file

@ -0,0 +1,622 @@
Panels:
- Class: rviz_common/Displays
Help Height: 87
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1
- /TF1/Frames1
Splitter Ratio: 0.49496981501579285
Tree Height: 778
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /display_contacts
Value: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
base_link_inertia:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
box1_place:
Alpha: 1
Show Axes: false
Show Trail: false
box2_place:
Alpha: 1
Show Axes: false
Show Trail: false
box3_place:
Alpha: 1
Show Axes: false
Show Trail: false
box4_place:
Alpha: 1
Show Axes: false
Show Trail: false
box5_place:
Alpha: 1
Show Axes: false
Show Trail: false
box6_place:
Alpha: 1
Show Axes: false
Show Trail: false
flange:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ft_frame:
Alpha: 1
Show Axes: false
Show Trail: false
inner_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
outer_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_inner_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_inner_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_grasp_point:
Alpha: 1
Show Axes: false
Show Trail: false
shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
tool0:
Alpha: 1
Show Axes: false
Show Trail: false
upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base:
Value: false
base_link:
Value: false
base_link_inertia:
Value: false
box1:
Value: false
box1_place:
Value: false
box2:
Value: false
box2_place:
Value: false
box3:
Value: false
box3_place:
Value: false
box4:
Value: false
box4_place:
Value: false
box5:
Value: false
box5_place:
Value: false
box6:
Value: false
box6_place:
Value: false
flange:
Value: false
forearm_link:
Value: false
fork_gt:
Value: true
ft_frame:
Value: false
inner_rgbd_camera:
Value: false
outer_rgbd_camera:
Value: false
robotiq_85_base_link:
Value: false
robotiq_85_left_finger_link:
Value: false
robotiq_85_left_finger_tip_link:
Value: false
robotiq_85_left_inner_knuckle_link:
Value: false
robotiq_85_left_knuckle_link:
Value: false
robotiq_85_right_finger_link:
Value: false
robotiq_85_right_finger_tip_link:
Value: false
robotiq_85_right_inner_knuckle_link:
Value: false
robotiq_85_right_knuckle_link:
Value: false
robotiq_grasp_point:
Value: false
shoulder_link:
Value: false
tool0:
Value: false
upper_arm_link:
Value: false
world:
Value: false
wrist_1_link:
Value: false
wrist_2_link:
Value: false
wrist_3_link:
Value: false
Marker Scale: 0.4000000059604645
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
world:
base_link:
base:
{}
base_link_inertia:
shoulder_link:
upper_arm_link:
forearm_link:
wrist_1_link:
wrist_2_link:
wrist_3_link:
flange:
tool0:
inner_rgbd_camera:
{}
robotiq_85_base_link:
robotiq_85_left_inner_knuckle_link:
{}
robotiq_85_left_knuckle_link:
robotiq_85_left_finger_link:
robotiq_85_left_finger_tip_link:
{}
robotiq_85_right_inner_knuckle_link:
{}
robotiq_85_right_knuckle_link:
robotiq_85_right_finger_link:
robotiq_85_right_finger_tip_link:
{}
robotiq_grasp_point:
{}
ft_frame:
{}
box1:
{}
box1_place:
{}
box2:
{}
box2_place:
{}
box3:
{}
box3_place:
{}
box4:
{}
box4_place:
{}
box5:
{}
box5_place:
{}
box6:
{}
box6_place:
{}
fork_gt:
{}
outer_rgbd_camera:
{}
Update Interval: 0
Value: true
- Class: moveit_rviz_plugin/Trajectory
Color Enabled: false
Enabled: true
Interrupt Display: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
base_link_inertia:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
box1_place:
Alpha: 1
Show Axes: false
Show Trail: false
box2_place:
Alpha: 1
Show Axes: false
Show Trail: false
box3_place:
Alpha: 1
Show Axes: false
Show Trail: false
box4_place:
Alpha: 1
Show Axes: false
Show Trail: false
box5_place:
Alpha: 1
Show Axes: false
Show Trail: false
box6_place:
Alpha: 1
Show Axes: false
Show Trail: false
flange:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ft_frame:
Alpha: 1
Show Axes: false
Show Trail: false
inner_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
outer_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_inner_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_left_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_inner_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_85_right_knuckle_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
robotiq_grasp_point:
Alpha: 1
Show Axes: false
Show Trail: false
shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
tool0:
Alpha: 1
Show Axes: false
Show Trail: false
upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Loop Animation: false
Name: Trajectory
Robot Alpha: 0.5
Robot Color: 150; 50; 150
Robot Description: robot_description
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 3x
Trail Step Size: 1
Trajectory Topic: /display_planned_path
Use Sim Time: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 6.619869709014893
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.2489434778690338
Y: -0.013962505385279655
Z: 0.13800443708896637
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.4353981614112854
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.2503976821899414
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 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
Selection:
collapsed: false
Tool Properties:
collapsed: false
Trajectory - Trajectory Slider:
collapsed: false
Views:
collapsed: false
Width: 1850
X: 70
Y: 27

454
rbs_bringup/config/rbs.rviz Normal file
View file

@ -0,0 +1,454 @@
Panels:
- Class: rviz_common/Displays
Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1/Frames1
- /MarkerArray2
Splitter Ratio: 0.49496981501579285
Tree Height: 981
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /display_contacts
Value: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
base_link_inertia:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
flange:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ft_frame:
Alpha: 1
Show Axes: false
Show Trail: false
inner_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
outer_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_grasp_point:
Alpha: 1
Show Axes: false
Show Trail: false
schunk_gripper_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
tool0:
Alpha: 1
Show Axes: false
Show Trail: false
upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base:
Value: false
base_link:
Value: false
base_link_inertia:
Value: false
flange:
Value: false
forearm_link:
Value: false
ft_frame:
Value: false
inner_rgbd_camera:
Value: false
outer_rgbd_camera:
Value: false
schunk_grasp_point:
Value: true
schunk_gripper_base_link:
Value: true
schunk_l_finger_link:
Value: true
schunk_r_finger_link:
Value: true
shoulder_link:
Value: false
tool0:
Value: false
upper_arm_link:
Value: false
world:
Value: false
wrist_1_link:
Value: false
wrist_2_link:
Value: false
wrist_3_link:
Value: false
Marker Scale: 0.4000000059604645
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
world:
base_link:
base:
{}
base_link_inertia:
shoulder_link:
upper_arm_link:
forearm_link:
wrist_1_link:
wrist_2_link:
wrist_3_link:
flange:
tool0:
inner_rgbd_camera:
{}
schunk_grasp_point:
{}
schunk_gripper_base_link:
schunk_l_finger_link:
{}
schunk_r_finger_link:
{}
ft_frame:
{}
outer_rgbd_camera:
{}
Update Interval: 0
Value: true
- Class: moveit_rviz_plugin/Trajectory
Color Enabled: false
Enabled: true
Interrupt Display: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
base_link_inertia:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
flange:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
ft_frame:
Alpha: 1
Show Axes: false
Show Trail: false
inner_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
outer_rgbd_camera:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_grasp_point:
Alpha: 1
Show Axes: false
Show Trail: false
schunk_gripper_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
schunk_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
tool0:
Alpha: 1
Show Axes: false
Show Trail: false
upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Loop Animation: false
Name: Trajectory
Robot Alpha: 0.5
Robot Color: 150; 50; 150
Robot Description: robot_description
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 3x
Trail Step Size: 1
Trajectory Topic: /display_planned_path
Use Sim Time: false
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: workspace
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 6.619869709014893
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.2489434778690338
Y: -0.013962505385279655
Z: 0.13800443708896637
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5753980875015259
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 1.1453965902328491
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1385
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 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
Selection:
collapsed: false
Tool Properties:
collapsed: false
Trajectory - Trajectory Slider:
collapsed: false
Views:
collapsed: false
Width: 2307
X: 436
Y: 66

37
rbs_bringup/launch/bringup.launch.py
View file

@ -9,6 +9,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
# from rbs_launch_utils.launch_common import load_yaml_abs
def generate_launch_description():
declared_arguments = []
@ -49,6 +50,13 @@ def generate_launch_description():
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"controllers_with_gripper_file",
default_value="ur_plus_gripper_controllers.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
@ -146,7 +154,7 @@ def generate_launch_description():
DeclareLaunchArgument("launch_perception", default_value="false", description="Launch perception?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner", default_value="true", description="Launch task_planner?")
DeclareLaunchArgument("launch_task_planner", default_value="false", description="Launch task_planner?")
)
declared_arguments.append(
DeclareLaunchArgument("cartesian_controllers", default_value="false", description="Load cartesian controllers?")
@ -185,12 +193,9 @@ def generate_launch_description():
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"]
)
mujoco_model = PathJoinSubstitution(
[FindPackageShare("rbs_simulation"), "mujoco_model", "current_mj.xml"]
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
)
robot_description_content = Command(
[
@ -211,7 +216,6 @@ def generate_launch_description():
"sim_fake:=", sim_fake, " ",
"simulation_controllers:=", initial_joint_controllers_file_path, " ",
"with_gripper:=", with_gripper_condition, " ",
"mujoco_model:=", mujoco_model,
]
)
robot_description = {"robot_description": robot_description_content}
@ -303,6 +307,24 @@ def generate_launch_description():
'robot_description_semantic': robot_description_semantic_content
}.items(),
condition=IfCondition(launch_moveit))
skills = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_skill_servers'),
'launch',
'skills.launch.py'
])
]),
launch_arguments={
'robot_description': robot_description_content,
'robot_description_semantic': robot_description_semantic_content,
'robot_description_kinematics': robot_description_kinematics,
'use_sim_time': use_sim_time,
'with_gripper_condition': with_gripper_condition,
'points_params_filepath': "gripperPositions.yaml"
}.items()
)
task_planner = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
@ -336,6 +358,7 @@ def generate_launch_description():
control,
simulation,
moveit,
skills,
task_planner,
perception
]

0
env_manager/include/component_manager/component_template.hpp → rbs_bringup/rbs_launch_utils/__init__.py
View file

84
rbs_bringup/rbs_launch_utils/launch_common.py Normal file
View file

@ -0,0 +1,84 @@
#!/usr/bin/env python3
# Copyright (c) 2021 PickNik, Inc.
#
# 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.
#
# Author: Lovro Ivanov
import math
import os
import yaml
from ament_index_python.packages import get_package_share_directory
def construct_angle_radians(loader, node):
"""Utility function to construct radian values from yaml."""
value = loader.construct_scalar(node)
try:
return float(value)
except SyntaxError:
raise Exception("invalid expression: %s" % value)
def construct_angle_degrees(loader, node):
"""Utility function for converting degrees into radians from yaml."""
return math.radians(construct_angle_radians(loader, node))
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml_abs(absolute_file_path):
try:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None

27
rbs_bt_executor/CMakeLists.txt
View file

@ -9,7 +9,6 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
@ -18,7 +17,12 @@ find_package(ament_index_cpp REQUIRED)
find_package(rbs_skill_interfaces REQUIRED)
find_package(behavior_tree REQUIRED)
find_package(control_msgs REQUIRED)
find_package(component_interfaces REQUIRED)
find_package(lifecycle_msgs REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(env_manager_interfaces REQUIRED)
find_package(rbs_utils REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_ros REQUIRED)
if (NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
@ -28,7 +32,6 @@ set(dependencies
rclcpp
rclcpp_action
geometry_msgs
tf2_geometry_msgs
moveit_msgs
moveit_core
moveit_ros_planning
@ -36,9 +39,13 @@ set(dependencies
ament_index_cpp
rbs_skill_interfaces
behavior_tree
std_msgs
control_msgs
component_interfaces
lifecycle_msgs
rcl_interfaces
env_manager_interfaces
rbs_utils
tf2_ros
tf2_eigen
)
include_directories(include)
@ -61,8 +68,14 @@ list(APPEND plugin_libs rbs_add_planning_scene_object)
add_library(rbs_assemble_process_state SHARED src/AssembleProcessState.cpp)
list(APPEND plugin_libs rbs_assemble_process_state)
add_library(rbs_detect_object SHARED src/DetectObject.cpp)
list(APPEND plugin_libs rbs_detect_object)
add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp)
list(APPEND plugin_libs rbs_pose_estimation)
add_library(rbs_env_manager_starter SHARED src/EnvManager.cpp)
list(APPEND plugin_libs rbs_env_manager_starter)
add_library(rbs_skill_move_topose_array_bt_action_client SHARED src/MoveToPoseArray.cpp)
list(APPEND plugin_libs rbs_skill_move_topose_array_bt_action_client)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})

14
rbs_bt_executor/bt_trees/test_gripper.xml Normal file
View file

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
</root>

193
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -1,155 +1,56 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<!-- ////////// -->
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="GetPickPlacePoses"
ObjectName="box1"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box2"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box3"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box4"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box5"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box6"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
<SubTreePlus ID="WorkspaceInspection" __autoremap="1" goal_pose="{workspace}"
robot_name="ur_manipulator" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="WorkspaceInspection">
<Sequence>
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_topose" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="MoveToPose">
<input_port name="robot_name"/>
<input_port name="pose_name"/>
<Action ID="EnvStarter">
<inout_port name="env_class" />
<inout_port name="env_name" />
<inout_port name="server_name" />
<inout_port name="server_timeout" />
<inout_port name="workspace" />
</Action>
<Action ID="MoveToPose">
<inout_port name="cancel_timeout" />
<inout_port name="pose" />
<inout_port name="robot_name" />
<inout_port name="server_name" />
<inout_port name="server_timeout" />
</Action>
<Action ID="MoveToPoseArray">
<inout_port name="cancel_timeout" />
<inout_port name="pose_vec_in" />
<inout_port name="robot_name" />
<inout_port name="server_name" />
<inout_port name="server_timeout" />
</Action>
<SubTree ID="PoseEstimation" />
<SubTree ID="WorkspaceInspection" />
</TreeNodesModel>
</root>
<!-- ////////// -->
</root>

12
rbs_bt_executor/bt_trees/test_tree_env_manager.xml Normal file
View file

@ -0,0 +1,12 @@
<?xml version="1.0"?>
<root main_tree_to_execute="EnvManagerBT">
<BehaviorTree ID="EnvManagerBT">
<Sequence>
<Action ID="EnvStarter"
env_name="gz_enviroment"
env_class="gz_enviroment::GzEnviroment"
server_name="/env_manager/start_env"
server_timeout="1000" />
</Sequence>
</BehaviorTree>
</root>

160
rbs_bt_executor/bt_trees/test_tree_main.xml Normal file
View file

@ -0,0 +1,160 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="GetPickPlacePoses"
ObjectName="box1"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose"
robot_name="ur_manipulator_gripper"
pose="{PreGraspPose}"
server_name="/move_topose"
server_timeout="10000"
cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box2"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box3"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box4"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box5"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box6"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="MoveToPose">
<input_port name="robot_name"/>
<input_port name="pose_name"/>
</Action>
</TreeNodesModel>
</root>

17
rbs_bt_executor/bt_trees/test_tree_pe_cleanup.xml Normal file
View file

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<root main_tree_to_execute="PoseEstimation">
<BehaviorTree ID="PoseEstimation">
<Sequence>
<Action ID="PoseEstimation"
ObjectName=""
ReqKind = "deactivate"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
<Action ID="PoseEstimation"
ObjectName=""
ReqKind = "cleanup"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

17
rbs_bt_executor/bt_trees/test_tree_pe_configure.xml Normal file
View file

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<root main_tree_to_execute="PoseEstimation">
<BehaviorTree ID="PoseEstimation">
<Sequence>
<Action ID="PoseEstimation"
ObjectName="fork"
ReqKind = "configure"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
<Action ID="PoseEstimation"
ObjectName=""
ReqKind = "activate"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

46
rbs_bt_executor/config/params.yaml
View file

@ -1,33 +1,13 @@
# Here is a nodes which calling from launch for bt_tree
simple_move_bt_nodes:
ros__parameters:
# plugins:
# - rbs_skill_move_topose_bt_action_client
pose1: [
0.11724797630931184, #X position
0.46766635768602544, #Y
0.5793862343094913, #Z
0.9987999001314066, #X orientation
0.041553846820940925, #Y
-0.004693314677006583, #Z
-0.025495295825239704 #W
]
pose2: [
-0.11661364861606047,
0.4492600889665261,
0.5591700913807053,
0.9962273179258467,
0.04057380155886888,
0.009225849745372298,
0.07615629548377048
]
pose3: [
-0.07133612047767886,
0.41038906578748613,
0.2844649846989331,
0.999078481789772,
0.04109234110437432,
0.006539754292177074,
0.010527978961032304
]
bt_engine:
ros__parameters:
plugins: [
"rbs_skill_move_topose_bt_action_client",
"rbs_skill_get_pick_place_pose_service_client",
"rbs_skill_gripper_move_bt_action_client",
"rbs_skill_move_joint_state",
"rbs_add_planning_scene_object",
"rbs_pose_estimation",
"rbs_env_manager_starter",
"rbs_skill_move_topose_array_bt_action_client"
]

97
rbs_bt_executor/launch/rbs_executor.launch.py
View file

@ -1,89 +1,38 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
import os
import yaml
import math
def construct_angle_radians(loader, node):
"""Utility function to construct radian values from yaml."""
value = loader.construct_scalar(node)
try:
return float(value)
except SyntaxError:
raise Exception("invalid expression: %s" % value)
def construct_angle_degrees(loader, node):
"""Utility function for converting degrees into radians from yaml."""
return math.radians(construct_angle_radians(loader, node))
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
def generate_launch_description():
bt_config = os.path.join(
get_package_share_directory('rbs_bt_executor'),
'config',
'params.yaml'
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"bt_file",
default_value="test_tree.xml",
description="BehaviorTree file for run.",
)
)
points = load_yaml(
package_name="rbs_bt_executor",
file_path="config/points.yaml"
bt_file = LaunchConfiguration("bt_file")
btfile_description = PathJoinSubstitution(
[FindPackageShare("rbs_bt_executor"), "bt_trees", bt_file]
)
btfile_param = {"bt_file_path": btfile_description}
bt_skills_param = PathJoinSubstitution([FindPackageShare("rbs_bt_executor"), "config", "params.yaml"])
gripperPoints = load_yaml(
package_name="rbs_bt_executor",
file_path="config/gripperPositions.yaml")
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare("ur_moveit_config"), "srdf", "ur.srdf.xacro"]
),
" ",
"name:=",
"ur",
" ",
"prefix:=",
"",
" ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
return LaunchDescription([
nodes_to_start = [
Node(
package='behavior_tree',
namespace='',
executable='bt_engine',
#prefix=['xterm -e gdb -ex run --args'],
# prefix=['gdbserver localhost:3000'],
parameters=[
{'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
{'plugins':['rbs_skill_move_topose_bt_action_client', "rbs_skill_get_pick_place_pose_service_client", "rbs_skill_gripper_move_bt_action_client", "rbs_skill_move_joint_state", "rbs_add_planning_scene_object"]},
gripperPoints,
robot_description_semantic
btfile_param,
bt_skills_param
]
)
])
]
return LaunchDescription(declared_arguments + nodes_to_start)

3
rbs_bt_executor/package.xml
View file

@ -8,9 +8,10 @@
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rbs_utils</depend>
<depend>rbs_skill_interfaces</depend>
<depend>component_interfaces</depend>
<depend>env_manager_interfaces</depend>
<depend>behavior_tree</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

63
rbs_bt_executor/src/AddPlanningSceneObject.cpp
View file

@ -1,45 +1,44 @@
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtService.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
using namespace BT;
using AddPlanningSceneObjectClient = rbs_skill_interfaces::srv::AddPlanningSceneObject;
using AddPlanningSceneObjectClient =
rbs_skill_interfaces::srv::AddPlanningSceneObject;
class GetPickPlacePose : public BtService<AddPlanningSceneObjectClient>
{
class GetPickPlacePose : public BtService<AddPlanningSceneObjectClient> {
public:
GetPickPlacePose(const std::string & name, const BT::NodeConfiguration & config)
: BtService<AddPlanningSceneObjectClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
: BtService<AddPlanningSceneObjectClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
AddPlanningSceneObjectClient::Request::SharedPtr populate_request() override
{
auto request = std::make_shared<AddPlanningSceneObjectClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
auto place_pose_ = getInput<std::vector<double>>("pose").value();
request->object_id = object_name_;
request->object_pose = place_pose_;
AddPlanningSceneObjectClient::Request::SharedPtr populate_request() override {
auto request = std::make_shared<AddPlanningSceneObjectClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
auto place_pose_ = getInput<std::vector<double>>("pose").value();
request->object_id = object_name_;
request->object_pose = place_pose_;
return request;
}
return request;
}
BT::NodeStatus handle_response(AddPlanningSceneObjectClient::Response::SharedPtr response) override
{
RCLCPP_INFO(_node->get_logger(), "Response %d", response->success);
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus handle_response(
AddPlanningSceneObjectClient::Response::SharedPtr response) override {
RCLCPP_INFO(_node->get_logger(), "Response %d", response->success);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::vector<double>>("pose"),
});
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::vector<double>>("pose"),
});
}
private:
std::string object_name_{};
std::string object_name_{};
};
BT_REGISTER_NODES(factory) {

36
rbs_bt_executor/src/AssembleProcessState.cpp
View file

@ -1,22 +1,19 @@
#include <string>
#include <behavior_tree/BtService.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include "rbs_skill_interfaces/srv/assemble_state.hpp"
using AssembleStateSrv = rbs_skill_interfaces::srv::AssembleState;
class AssembleState : public BtService<AssembleStateSrv>
{
class AssembleState : public BtService<AssembleStateSrv> {
public:
AssembleState(const std::string & name, const BT::NodeConfiguration &config)
: BtService<AssembleStateSrv>(name, config)
{
AssembleState(const std::string &name, const BT::NodeConfiguration &config)
: BtService<AssembleStateSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
AssembleStateSrv::Request::SharedPtr populate_request()
{
AssembleStateSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<AssembleStateSrv::Request>();
auto state_kind = getInput<std::string>("StateKind").value();
request->req_kind = -1;
@ -39,20 +36,18 @@ public:
return request;
}
BT::NodeStatus handle_response(AssembleStateSrv::Response::SharedPtr response)
{
BT::NodeStatus
handle_response(AssembleStateSrv::Response::SharedPtr response) {
// TODO: return bad status on validate process return false state
return (response->call_status = true)? BT::NodeStatus::SUCCESS: BT::NodeStatus::FAILURE;
return (response->call_status = true) ? BT::NodeStatus::SUCCESS
: BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("StateKind"),
BT::InputPort<std::string>("AssembleName"),
BT::InputPort<std::string>("PartName"),
BT::InputPort<std::string>("WorkspaceName")
});
static BT::PortsList providedPorts() {
return providedBasicPorts({BT::InputPort<std::string>("StateKind"),
BT::InputPort<std::string>("AssembleName"),
BT::InputPort<std::string>("PartName"),
BT::InputPort<std::string>("WorkspaceName")});
}
private:
@ -61,7 +56,6 @@ private:
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
BT_REGISTER_NODES(factory) {
factory.registerNodeType<AssembleState>("AssembleState");
}

51
rbs_bt_executor/src/DetectObject.cpp
View file

@ -1,51 +0,0 @@
#include <string>
#include <behavior_tree/BtService.hpp>
#include <rclcpp/rclcpp.hpp>
#include <component_interfaces/srv/detect_object.hpp>
using DetectObjectSrv = component_interfaces::srv::DetectObject;
class DetectObject: public BtService<DetectObjectSrv>
{
public:
DetectObject(const std::string &name, const BT::NodeConfiguration &config)
: BtService<DetectObjectSrv>(name, config)
{
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
DetectObjectSrv::Request::SharedPtr populate_request()
{
auto request = std::make_shared<DetectObjectSrv::Request>();
auto req_kind = getInput<std::string>("ReqKind").value();
if (req_kind == "DETECT")
request->req_kind = 0;
else if (req_kind == "FOLLOW")
request->req_kind = 1;
else
request->req_kind = 2;
request->object_name = getInput<std::string>("ObjectName").value();
return request;
}
BT::NodeStatus handle_response(DetectObjectSrv::Response::SharedPtr response)
{
return response->call_status? BT::NodeStatus::SUCCESS: BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>("ReqKind"),
BT::InputPort<std::string>("ObjectName"),
});
}
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<DetectObject>("DetectObject");
}

59
rbs_bt_executor/src/EnvManager.cpp Normal file
View file

@ -0,0 +1,59 @@
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <memory>
#include <rclcpp/logging.hpp>
#include <string>
#include "rbs_utils/rbs_utils.hpp"
#include "env_manager_interfaces/srv/start_env.hpp"
#include "env_manager_interfaces/srv/unload_env.hpp"
using EnvStarterService = env_manager_interfaces::srv::StartEnv;
class EnvManagerStarter : public BtService<EnvStarterService> {
public:
EnvManagerStarter(const std::string &name,
const BT::NodeConfiguration &config)
: BtService<EnvStarterService>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
EnvStarterService::Request::SharedPtr populate_request() override {
auto request = std::make_shared<EnvStarterService::Request>();
std::string env_name = getInput<std::string>("env_name").value();
std::string env_class = getInput<std::string>("env_class").value();
request->name = env_name;
request->type = env_class;
return request;
}
BT::NodeStatus handle_response(
const EnvStarterService::Response::SharedPtr response) override {
if (response->ok) {
// TODO We need better perfomance for call it in another place for all BT nodes
// - Make it via shared_ptr forward throught blackboard
auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2", _node);
auto p = t->getWorkspaceInspectorTrajectory();
setOutput("workspace", p);
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("env_name"),
BT::InputPort<std::string>("env_class"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("workspace"),
});
}
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<EnvManagerStarter>("EnvStarter");
}

170
rbs_bt_executor/src/GetPickPlacePoses.cpp
View file

@ -1,98 +1,102 @@
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtService.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose.hpp"
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
using namespace BT;
using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses;
class GetPickPlacePose : public BtService<GetPickPlacePoseClient>
{
class GetPickPlacePose : public BtService<GetPickPlacePoseClient> {
public:
GetPickPlacePose(const std::string & name, const BT::NodeConfiguration & config)
: BtService<GetPickPlacePoseClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
: BtService<GetPickPlacePoseClient>(name, config) {}
GetPickPlacePoseClient::Request::SharedPtr populate_request() override
{
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
grasp_direction_str_ = getInput<std::string>("GraspDirection").value();
place_direction_str_ = getInput<std::string>("PlaceDirection").value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Starting process for object: " << object_name_);
grasp_direction_ = convert_direction_from_string(grasp_direction_str_);
place_direction_ = convert_direction_from_string(place_direction_str_);
request->object_name = object_name_;
request->grasp_direction = grasp_direction_;
request->place_direction = place_direction_;
return request;
}
GetPickPlacePoseClient::Request::SharedPtr populate_request() override {
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
grasp_direction_str_ = getInput<std::string>("GraspDirection").value();
place_direction_str_ = getInput<std::string>("PlaceDirection").value();
RCLCPP_INFO_STREAM(_node->get_logger(),
"Starting process for object: " << object_name_);
grasp_direction_ = convert_direction_from_string(grasp_direction_str_);
place_direction_ = convert_direction_from_string(place_direction_str_);
request->object_name = object_name_;
request->grasp_direction = grasp_direction_;
request->place_direction = place_direction_;
return request;
}
BT::NodeStatus handle_response(GetPickPlacePoseClient::Response::SharedPtr response) override
{
RCLCPP_INFO(_node->get_logger(), "\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t orientation.w: %f",
response->grasp[2].position.x, response->grasp[2].position.y, response->grasp[2].position.z,
response->grasp[2].orientation.x, response->grasp[2].orientation.y, response->grasp[2].orientation.z, response->grasp[2].orientation.w);
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[3]);
setOutput<geometry_msgs::msg::Pose>("PostPostGraspPose", response->grasp[2]);
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[1]);
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus handle_response(
GetPickPlacePoseClient::Response::SharedPtr response) override {
// RCLCPP_INFO(_node->get_logger(),
// "\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
// "\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
// "orientation.z: %f \n\t orientation.w: %f",
// response->grasp[1].position.x, response->grasp[1].position.y,
// response->grasp[1].position.z, response->grasp[1].orientation.x,
// response->grasp[1].orientation.y,
// response->grasp[1].orientation.z,
// response->grasp[1].orientation.w);
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[2]);
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::string>("GraspDirection"), // x or y or z
InputPort<std::string>("PlaceDirection"), // x or y or z
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"), //TODO: change to std::vector<>
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
});
}
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[2]);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::string>("GraspDirection"), // x or y or z
InputPort<std::string>("PlaceDirection"), // x or y or z
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
// TODO: change to std::vector<>
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
});
}
geometry_msgs::msg::Vector3 convert_direction_from_string(std::string str) {
std::map<std::string, int> directions;
geometry_msgs::msg::Vector3 vector;
directions["x"] = 1;
directions["y"] = 2;
directions["z"] = 3;
switch (directions[str]) {
case 1:
vector.x = 1;
vector.y = 0;
vector.z = 0;
break;
case 2:
vector.x = 0;
vector.y = 1;
vector.z = 0;
break;
case 3:
vector.x = 0;
vector.y = 0;
vector.z = 1;
break;
};
return vector;
}
geometry_msgs::msg::Vector3 convert_direction_from_string(std::string str)
{
std::map<std::string, int> directions;
geometry_msgs::msg::Vector3 vector;
directions["x"] = 1;
directions["y"] = 2;
directions["z"] = 3;
switch(directions[str]){
case 1:
vector.x = 1;
vector.y = 0;
vector.z = 0;
break;
case 2:
vector.x = 0;
vector.y = 1;
vector.z = 0;
break;
case 3:
vector.x = 0;
vector.y = 0;
vector.z = 1;
break;
};
return vector;
}
private:
std::string object_name_{};
std::string grasp_direction_str_{};
std::string place_direction_str_{};
geometry_msgs::msg::Vector3 grasp_direction_{};
geometry_msgs::msg::Vector3 place_direction_{};
std::string object_name_{};
std::string grasp_direction_str_{};
std::string place_direction_str_{};
geometry_msgs::msg::Vector3 grasp_direction_{};
geometry_msgs::msg::Vector3 place_direction_{};
};
BT_REGISTER_NODES(factory) {

76
rbs_bt_executor/src/MoveToJointStates.cpp
View file

@ -1,62 +1,56 @@
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry"
#include "rclcpp/parameter.hpp"
using namespace BT;
using MoveToJointStateAction = rbs_skill_interfaces::action::MoveitSendJointStates;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToJointStateActionClient");
using MoveToJointStateAction =
rbs_skill_interfaces::action::MoveitSendJointStates;
static const rclcpp::Logger LOGGER =
rclcpp::get_logger("MoveToJointStateActionClient");
class MoveToJointState : public BtAction<MoveToJointStateAction>
{
public:
MoveToJointState(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToJointStateAction>(name, config)
{
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
class MoveToJointState : public BtAction<MoveToJointStateAction> {
public:
MoveToJointState(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToJointStateAction>(name, config) {}
MoveToJointStateAction::Goal populate_goal() override
{
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("PrePlaceJointState", joint_values_);
MoveToJointStateAction::Goal populate_goal() override {
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("JointState", joint_values_);
auto goal = MoveToJointStateAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]", robot_name_.c_str());
auto goal = MoveToJointStateAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]",
robot_name_.c_str());
for (auto joint : joint_values_)
{
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value " << joint);
}
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value size " << joint_values_.size());
goal.robot_name = robot_name_;
goal.joint_values = joint_values_;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
for (auto joint : joint_values_) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value " << joint);
}
RCLCPP_INFO_STREAM(_node->get_logger(),
"Joint value size " << joint_values_.size());
goal.robot_name = robot_name_;
goal.joint_values = joint_values_;
return goal;
}
RCLCPP_INFO(_node->get_logger(), "Goal populated");
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("PrePlaceJointState")
});
}
return goal;
}
private:
std::string robot_name_{};
std::vector<double> joint_values_;
static PortsList providedPorts() {
return providedBasicPorts(
{InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("JointState")});
}
private:
std::string robot_name_{};
std::vector<double> joint_values_;
}; // class MoveToJointState
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToJointState>("MoveToJointState");
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToJointState>("MoveToJointState");
}

91
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -1,68 +1,53 @@
// Copyright [2023] bill-finger
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
#include "behaviortree_cpp_v3/bt_factory.h"
// #include "Eigen/Geometry"
#include "rclcpp/parameter.hpp"
using namespace BT;
using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient");
class MoveToPose : public BtAction<MoveToPoseAction>
{
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config)
{
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
class MoveToPose : public BtAction<MoveToPoseAction> {
public:
MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToPoseAction>(name, config) {}
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::Pose>("pose", pose_des);
RCLCPP_INFO(_node->get_logger(), "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t orientation.w: %f",
pose_des.position.x, pose_des.position.y, pose_des.position.z,
pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w);
MoveToPoseAction::Goal populate_goal() override {
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::Pose>("pose", pose_des);
RCLCPP_INFO(_node->get_logger(),
"\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
"\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
"orientation.z: %f \n\t orientation.w: %f",
pose_des.position.x, pose_des.position.y, pose_des.position.z,
pose_des.orientation.x, pose_des.orientation.y,
pose_des.orientation.z, pose_des.orientation.w);
auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
goal.robot_name = robot_name_;
goal.target_pose = pose_des;
goal.end_effector_acceleration = 0.5;
goal.end_effector_velocity = 0.5;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]",
robot_name_.c_str());
return goal;
}
goal.robot_name = robot_name_;
goal.target_pose = pose_des;
goal.end_effector_acceleration = 0.5;
goal.end_effector_velocity = 0.5;
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("pose")
});
}
RCLCPP_INFO(_node->get_logger(), "Goal populated");
private:
std::string robot_name_;
//std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des;
//std::map<std::string, geometry_msgs::msg::Pose> Poses;
return goal;
}
// geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){
static BT::PortsList providedPorts() {
return providedBasicPorts(
{BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::Pose>("pose")});
}
// }
private:
std::string robot_name_;
geometry_msgs::msg::Pose pose_des;
}; // class MoveToPose
}; // class MoveToPose
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToPose>("MoveToPose");
}
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPose>("MoveToPose");
}

56
rbs_bt_executor/src/MoveToPoseArray.cpp Normal file
View file

@ -0,0 +1,56 @@
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "geometry_msgs/msg/pose_array.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_utils/rbs_utils.hpp"
#include <behaviortree_cpp_v3/bt_factory.h>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>
using MoveToPoseArrayAction = rbs_skill_interfaces::action::MoveitSendPose;
class MoveToPoseArray : public BtAction<MoveToPoseArrayAction> {
public:
MoveToPoseArray(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToPoseArrayAction>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
MoveToPoseArrayAction::Goal populate_goal() override {
auto goal = MoveToPoseArrayAction::Goal();
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::PoseArray>("pose_vec_in", target_pose_vec_);
for (auto &point : target_pose_vec_.poses) {
RCLCPP_INFO(_node->get_logger(), "Pose array: \n %f \n %f \n %f",
point.position.x, point.position.y, point.position.z);
}
if (!target_pose_vec_.poses.empty()) {
goal.robot_name = robot_name_;
goal.target_pose = target_pose_vec_.poses.at(0);
target_pose_vec_.poses.erase(target_pose_vec_.poses.begin());
setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out", target_pose_vec_);
} else {
RCLCPP_WARN(_node->get_logger(), "Target pose vector empty");
}
return goal;
}
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")
});
}
private:
std::string robot_name_;
geometry_msgs::msg::PoseArray target_pose_vec_;
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPoseArray>("MoveToPoseArray");
}

124
rbs_bt_executor/src/PoseEstimation.cpp Normal file
View file

@ -0,0 +1,124 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/logging.hpp>
#include <string>
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
class PoseEstimation : public BtService<PoseEstimationSrv> {
public:
PoseEstimation(const std::string &name, const BT::NodeConfiguration &config)
: BtService<PoseEstimationSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
_node, "/pose_estimation");
while (!_set_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(),
"Interrupted while waiting for the service. Exiting.");
break;
}
RCLCPP_WARN(_node->get_logger(),
"service not available, waiting again...");
}
_model_name = getInput<std::string>("ObjectName").value();
}
PoseEstimationSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<PoseEstimationSrv::Request>();
_req_type = getInput<std::string>("ReqKind").value();
request->set__transition(transition_event(_req_type));
return request;
}
BT::NodeStatus
handle_response(const PoseEstimationSrv::Response::SharedPtr response) {
if (response->success) {
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("ReqKind"),
BT::InputPort<std::string>("ObjectName"),
BT::InputPort<std::string>("ObjectPath"),
});
}
private:
uint8_t transition_id_{};
std::string _model_name{};
std::string _model_path{};
std::string _req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
std::vector<rcl_interfaces::msg::Parameter> _params;
rcl_interfaces::msg::Parameter _param;
lifecycle_msgs::msg::Transition
transition_event(const std::string &req_type) {
lifecycle_msgs::msg::Transition translation{};
if (req_type == "configure") {
set_mesh_param();
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
} else if (req_type == "calibrate") {
set_str_param();
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
} else if (req_type == "activate") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
} else if (req_type == "deactivate") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
} else if (req_type == "cleanup") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
}
// calibrate
translation.set__id(transition_id_);
return translation;
}
inline std::string build_model_path(const std::string &model_name,
const std::string &package_path) {
return package_path + "/config/" + model_name + ".ply";
}
inline std::string build_model_path(const std::string &model_path) {
return model_path;
}
void set_str_param() {
RCLCPP_INFO_STREAM(_node->get_logger(),
"Set string parameter: <" + _model_name + ">");
std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("mesh_path", _model_name)};
_set_params_client->set_parameters(_parameters);
}
void set_mesh_param() {
auto _package_name =
ament_index_cpp::get_package_share_directory("rbs_perception");
_model_path = build_model_path(_model_name, _package_name);
RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("mesh_path", _model_name)};
_set_params_client->set_parameters(_parameters);
}
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<PoseEstimation>("PoseEstimation");
}

Some files were not shown because too many files have changed in this diff Show more