Merge remote-tracking branch 'origin/83-env-manager-refactoring' into 90-real-robot-setup
This commit is contained in:
commit
fe94fc7bc2
78 changed files with 1860 additions and 2377 deletions
|
|
@ -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()
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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", "fork_gt"};
|
|
||||||
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)
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -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.
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
### 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.
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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_tf2_broadcaster_component.so",
|
|
||||||
class = "scene_component::IgnTf2Broadcaster"
|
|
||||||
},
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -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"
|
|
||||||
},
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -1,12 +0,0 @@
|
||||||
NODES = {
|
|
||||||
camera_node = {
|
|
||||||
name = "camera_node",
|
|
||||||
type = "Publisher",
|
|
||||||
msg_type = "sensor_msgs/Image"
|
|
||||||
},
|
|
||||||
scene_component = {
|
|
||||||
name = "scene_component",
|
|
||||||
type = "Publisher",
|
|
||||||
msg_type = "tf2_msgs/tf_message"
|
|
||||||
},
|
|
||||||
}
|
|
||||||
|
|
@ -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
|
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 176 KiB |
56
env_manager/env_interface/CMakeLists.txt
Normal file
56
env_manager/env_interface/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,56 @@
|
||||||
|
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
|
||||||
|
)
|
||||||
|
|
||||||
|
# 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()
|
||||||
|
|
@ -1,3 +1,4 @@
|
||||||
|
|
||||||
Apache License
|
Apache License
|
||||||
Version 2.0, January 2004
|
Version 2.0, January 2004
|
||||||
http://www.apache.org/licenses/
|
http://www.apache.org/licenses/
|
||||||
|
|
@ -186,7 +187,7 @@
|
||||||
same "printed page" as the copyright notice for easier
|
same "printed page" as the copyright notice for easier
|
||||||
identification within third-party archives.
|
identification within third-party archives.
|
||||||
|
|
||||||
Copyright 2022 Robossembler
|
Copyright [yyyy] [name of copyright owner]
|
||||||
|
|
||||||
Licensed under the Apache License, Version 2.0 (the "License");
|
Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
you may not use this file except in compliance with 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.
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
See the License for the specific language governing permissions and
|
See the License for the specific language governing permissions and
|
||||||
limitations under the License.
|
limitations under the License.
|
||||||
|
|
||||||
|
|
@ -0,0 +1,11 @@
|
||||||
|
#include "env_interface/env_interface_base.hpp"
|
||||||
|
|
||||||
|
namespace env_interface
|
||||||
|
{
|
||||||
|
class EnvInterface : public EnvInterfaceBase
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
EnvInterface() = default;
|
||||||
|
virtual ~EnvInterface() = default;
|
||||||
|
};
|
||||||
|
} // namespace env_interface
|
||||||
|
|
@ -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
|
||||||
|
|
@ -1,18 +1,16 @@
|
||||||
<?xml version="1.0"?>
|
<?xml version="1.0"?>
|
||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
<package format="3">
|
<package format="3">
|
||||||
<name>component_interfaces</name>
|
<name>env_interface</name>
|
||||||
<version>0.0.0</version>
|
<version>0.0.0</version>
|
||||||
<description>TODO: Package description</description>
|
<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>
|
<license>Apache-2.0</license>
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
<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>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
<export>
|
<export>
|
||||||
<build_type>ament_cmake</build_type>
|
<build_type>ament_cmake</build_type>
|
||||||
45
env_manager/env_interface/src/env_interface_base.cpp
Normal file
45
env_manager/env_interface/src/env_interface_base.cpp
Normal 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
77
env_manager/env_manager/CMakeLists.txt
Normal 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
202
env_manager/env_manager/LICENSE
Normal 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.
|
||||||
9
env_manager/env_manager/config/test_params.yaml
Normal file
9
env_manager/env_manager/config/test_params.yaml
Normal file
|
|
@ -0,0 +1,9 @@
|
||||||
|
env_manager:
|
||||||
|
ros__parameters:
|
||||||
|
gz_enviroment:
|
||||||
|
type: gz_enviroment/GzEnviroment
|
||||||
|
|
||||||
|
|
||||||
|
gz_enviroment:
|
||||||
|
ros__parameters:
|
||||||
|
mesh_name: "monkey"
|
||||||
86
env_manager/env_manager/include/env_manager/env_manager.hpp
Normal file
86
env_manager/env_manager/include/env_manager/env_manager.hpp
Normal file
|
|
@ -0,0 +1,86 @@
|
||||||
|
#ifndef __ENV_MANAGER_HPP__
|
||||||
|
#define __ENV_MANAGER_HPP__
|
||||||
|
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||||
|
#include "env_manager_interfaces/srv/load_env.hpp"
|
||||||
|
#include "env_manager_interfaces/srv/configure_env.hpp"
|
||||||
|
#include "env_manager_interfaces/srv/unload_env.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 & json_config_path,
|
||||||
|
// 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 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::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
23
env_manager/env_manager/package.xml
Normal 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_manager_interfaces</depend>
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>rclcpp_lifecycle</depend>
|
||||||
|
<depend>env_inteface</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
172
env_manager/env_manager/src/env_manager.cpp
Normal file
172
env_manager/env_manager/src/env_manager.cpp
Normal file
|
|
@ -0,0 +1,172 @@
|
||||||
|
#include "env_manager/env_manager.hpp"
|
||||||
|
#include "nlohmann/json.hpp"
|
||||||
|
|
||||||
|
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 & json_config_path,
|
||||||
|
// 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<rclcpp_lifecycle::LifecycleNode>>(
|
||||||
|
// 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));
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp::Node::SharedPtr EnvManager::getNode() { return m_node; }
|
||||||
|
|
||||||
|
void EnvManager::loadEnv_cb(
|
||||||
|
const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
|
||||||
|
env_manager_interfaces::srv::LoadEnv::Response::SharedPtr response) {
|
||||||
|
std::lock_guard<std::mutex> lock(m_env_mutex);
|
||||||
|
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) {
|
||||||
|
std::lock_guard<std::mutex> guard(m_env_mutex);
|
||||||
|
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) {
|
||||||
|
std::lock_guard<std::mutex> lock(m_env_mutex);
|
||||||
|
response->ok = unloadEnv(request->name) == EnvStateReturnType::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
EnvInterfaceSharedPtr EnvManager::addEnv(const EnvSpec &enviroment) {
|
||||||
|
std::lock_guard<std::mutex> lock(m_env_mutex);
|
||||||
|
// 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()) {
|
||||||
|
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) {
|
||||||
|
std::lock_guard<std::mutex> guard(m_env_mutex);
|
||||||
|
|
||||||
|
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()->configure();
|
||||||
|
// it->env_ptr->activate();
|
||||||
|
it->env_ptr->getNode()->activate();
|
||||||
|
|
||||||
|
return EnvStateReturnType::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace env_manager
|
||||||
21
env_manager/env_manager/src/run_env_manager.cpp
Normal file
21
env_manager/env_manager/src/run_env_manager.cpp
Normal 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;
|
||||||
|
}
|
||||||
36
env_manager/env_manager_interfaces/CMakeLists.txt
Normal file
36
env_manager/env_manager_interfaces/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,36 @@
|
||||||
|
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/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
202
env_manager/env_manager_interfaces/LICENSE
Normal 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
4
env_manager/env_manager_interfaces/msg/EnvState.msg
Normal 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
27
env_manager/env_manager_interfaces/package.xml
Normal 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
3
env_manager/env_manager_interfaces/srv/ConfigureEnv.srv
Normal file
|
|
@ -0,0 +1,3 @@
|
||||||
|
string name
|
||||||
|
---
|
||||||
|
bool ok
|
||||||
4
env_manager/env_manager_interfaces/srv/LoadEnv.srv
Normal file
4
env_manager/env_manager_interfaces/srv/LoadEnv.srv
Normal file
|
|
@ -0,0 +1,4 @@
|
||||||
|
string name
|
||||||
|
string type
|
||||||
|
---
|
||||||
|
bool ok
|
||||||
3
env_manager/env_manager_interfaces/srv/UnloadEnv.srv
Normal file
3
env_manager/env_manager_interfaces/srv/UnloadEnv.srv
Normal file
|
|
@ -0,0 +1,3 @@
|
||||||
|
string name
|
||||||
|
---
|
||||||
|
bool ok
|
||||||
71
env_manager/gz_enviroment/CMakeLists.txt
Normal file
71
env_manager/gz_enviroment/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,71 @@
|
||||||
|
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
|
||||||
|
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
202
env_manager/gz_enviroment/LICENSE
Normal 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
7
env_manager/gz_enviroment/gz_enviroment.xml
Normal 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>
|
||||||
|
|
@ -0,0 +1,47 @@
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||||
|
// #include "ros_gz_bridge/convert.hpp"
|
||||||
|
#include <string>
|
||||||
|
#include <tf2_ros/transform_broadcaster.h>
|
||||||
|
#include <tf2_msgs/msg/tf_message.hpp>
|
||||||
|
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||||
|
#include <gz/transport/Node.hh>
|
||||||
|
#include "gz/msgs/pose_v.pb.h"
|
||||||
|
#include "env_interface/env_interface.hpp"
|
||||||
|
|
||||||
|
#include <gz/sim/components/Model.hh>
|
||||||
|
#include <gz/sim/Entity.hh>
|
||||||
|
#include <gz/sim/EntityComponentManager.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::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::string getGzWorldName();
|
||||||
|
std::string createGzModelString(const std::vector<double>& pose, const std::string& mesh_filepath,
|
||||||
|
const std::string& name);
|
||||||
|
};
|
||||||
|
} // namespace gz_enviroment
|
||||||
25
env_manager/gz_enviroment/package.xml
Normal file
25
env_manager/gz_enviroment/package.xml
Normal 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>
|
||||||
180
env_manager/gz_enviroment/src/gz_enviroment.cpp
Normal file
180
env_manager/gz_enviroment/src/gz_enviroment.cpp
Normal file
|
|
@ -0,0 +1,180 @@
|
||||||
|
#include "gz_enviroment/gz_enviroment.hpp"
|
||||||
|
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||||
|
#include "ros_gz_bridge/convert.hpp"
|
||||||
|
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
||||||
|
#include <memory>
|
||||||
|
#include <rclcpp/logging.hpp>
|
||||||
|
#include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
|
||||||
|
|
||||||
|
#include <gz/sim/components.hh>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
std::cout << "Model Name: " << modelName->Data() << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
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";
|
||||||
|
|
||||||
|
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());
|
||||||
|
|
||||||
|
return CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&)
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup");
|
||||||
|
|
||||||
|
m_tf2_broadcaster.reset();
|
||||||
|
m_gz_node.reset();
|
||||||
|
|
||||||
|
return CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State&)
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate");
|
||||||
|
|
||||||
|
return CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO: Check 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", "monkey" };
|
||||||
|
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;
|
||||||
|
|
||||||
|
m_tf2_broadcaster->sendTransform(t);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
// TODO: From Config
|
||||||
|
std::vector<double> pps{ 1.0, 0.0, 1.0, 0.0, 0.0 };
|
||||||
|
std::string mesh_file = "monkey.stl";
|
||||||
|
std::string mesh_name = "monkey";
|
||||||
|
// ENDTODO
|
||||||
|
std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name);
|
||||||
|
req.set_sdf(sdf_string);
|
||||||
|
bool 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::string& mesh_filepath,
|
||||||
|
const std::string& name)
|
||||||
|
{
|
||||||
|
std::string model_template = std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
|
||||||
|
"<link name='link_" + name + "'>" + "<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>" +
|
||||||
|
"<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);
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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__
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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
26
env_manager/planning_scene_manager/CMakeLists.txt
Normal 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
202
env_manager/planning_scene_manager/LICENSE
Normal 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.
|
||||||
|
|
@ -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__
|
||||||
|
|
@ -1,16 +1,13 @@
|
||||||
<?xml version="1.0"?>
|
<?xml version="1.0"?>
|
||||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
<package format="3">
|
<package format="3">
|
||||||
<name>env_manager</name>
|
<name>planning_scene_manager</name>
|
||||||
<version>0.0.0</version>
|
<version>0.0.0</version>
|
||||||
<description>env_manager</description>
|
<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>
|
<license>Apache-2.0</license>
|
||||||
|
|
||||||
<buildtool_depend>ament_cmake_ros</buildtool_depend>
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
<depend>boost</depend>
|
|
||||||
<depend>rcl</depend>
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
<test_depend>ament_lint_common</test_depend>
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -1,6 +0,0 @@
|
||||||
#include "env_manager/environment_node_manager.hpp"
|
|
||||||
|
|
||||||
namespace env_manager
|
|
||||||
{
|
|
||||||
|
|
||||||
} // namespace env_manager
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -142,7 +142,7 @@ def generate_launch_description():
|
||||||
DeclareLaunchArgument("sim_fake", default_value="false", description="Mock contollers")
|
DeclareLaunchArgument("sim_fake", default_value="false", description="Mock contollers")
|
||||||
)
|
)
|
||||||
declared_arguments.append(
|
declared_arguments.append(
|
||||||
DeclareLaunchArgument("env_manager", default_value="true", description="Launch env_manager?")
|
DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
|
||||||
)
|
)
|
||||||
declared_arguments.append(
|
declared_arguments.append(
|
||||||
DeclareLaunchArgument("launch_sim", default_value="true", description="Launch simulator (Gazebo)? Most general arg")
|
DeclareLaunchArgument("launch_sim", default_value="true", description="Launch simulator (Gazebo)? Most general arg")
|
||||||
|
|
|
||||||
|
|
@ -20,7 +20,7 @@ def generate_launch_description():
|
||||||
"prefix": "",
|
"prefix": "",
|
||||||
"sim_mujoco": "true",
|
"sim_mujoco": "true",
|
||||||
"simulation_controllers": str(initial_joint_controllers_file_path),
|
"simulation_controllers": str(initial_joint_controllers_file_path),
|
||||||
"with_gripper": "false"
|
"with_gripper": "true"
|
||||||
})
|
})
|
||||||
|
|
||||||
robot_desc = doc.toprettyxml(indent=' ')
|
robot_desc = doc.toprettyxml(indent=' ')
|
||||||
|
|
|
||||||
|
|
@ -26,7 +26,7 @@ def generate_launch_description():
|
||||||
DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
|
DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
|
||||||
)
|
)
|
||||||
declared_arguments.append(
|
declared_arguments.append(
|
||||||
DeclareLaunchArgument("gazebo_gui", default_value="false", description="Launch env_manager?")
|
DeclareLaunchArgument("gazebo_gui", default_value="true", description="Launch env_manager?")
|
||||||
)
|
)
|
||||||
|
|
||||||
sim_gazebo = LaunchConfiguration("sim_gazebo")
|
sim_gazebo = LaunchConfiguration("sim_gazebo")
|
||||||
|
|
|
||||||
|
|
@ -69,4 +69,4 @@ int main(int argc, char **argv) {
|
||||||
rclcpp::shutdown();
|
rclcpp::shutdown();
|
||||||
g_node = nullptr;
|
g_node = nullptr;
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -116,4 +116,4 @@ RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
|
||||||
// rbs_skill_actions::GripperControlActionServer server;
|
// rbs_skill_actions::GripperControlActionServer server;
|
||||||
// rclcpp::spin(server);
|
// rclcpp::spin(server);
|
||||||
// return 0;
|
// return 0;
|
||||||
// }
|
// }
|
||||||
|
|
|
||||||
|
|
@ -178,4 +178,4 @@ int main(int argc, char **argv) {
|
||||||
run_server.join();
|
run_server.join();
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -160,4 +160,4 @@ int main(int argc, char **argv) {
|
||||||
run_server.join();
|
run_server.join();
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -172,4 +172,4 @@ int main(int argc, char **argv) {
|
||||||
run_server.join();
|
run_server.join();
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue