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
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
|
@ -186,7 +187,7 @@
|
|||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright 2022 Robossembler
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
|
|
@ -199,4 +200,3 @@
|
|||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
|
||||
|
|
@ -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-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>component_interfaces</name>
|
||||
<name>env_interface</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
|
||||
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<depend>std_msgs</depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<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-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>env_manager</name>
|
||||
<name>planning_scene_manager</name>
|
||||
<version>0.0.0</version>
|
||||
<description>env_manager</description>
|
||||
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="ur.narmak@gmail.com">Bill Finger</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake_ros</buildtool_depend>
|
||||
|
||||
<depend>boost</depend>
|
||||
<depend>rcl</depend>
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
|
@ -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")
|
||||
)
|
||||
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(
|
||||
DeclareLaunchArgument("launch_sim", default_value="true", description="Launch simulator (Gazebo)? Most general arg")
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ def generate_launch_description():
|
|||
"prefix": "",
|
||||
"sim_mujoco": "true",
|
||||
"simulation_controllers": str(initial_joint_controllers_file_path),
|
||||
"with_gripper": "false"
|
||||
"with_gripper": "true"
|
||||
})
|
||||
|
||||
robot_desc = doc.toprettyxml(indent=' ')
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ def generate_launch_description():
|
|||
DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
|
||||
)
|
||||
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")
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue