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

remove env_manager for refactoring

Browse source
This commit is contained in:
Ilya Uraev 2023-09-16 19:16:30 +03:00
parent 4371dbdcee
commit 8596ff0011
42 changed files with 0 additions and 2596 deletions
Download patch file Download diff file Expand all files Collapse all files

68
env_components/camera_component/CMakeLists.txt
View file

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

30
env_components/camera_component/package.xml
View file

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

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

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

16
env_components/component_interfaces/CMakeLists.txt
View file

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

20
env_components/component_interfaces/package.xml
View file

@ -1,20 +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>component_interfaces</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</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>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

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

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

118
env_components/scene_component/CMakeLists.txt
View file

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

31
env_components/scene_component/package.xml
View file

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

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

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

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

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

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

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

144
env_manager/CMakeLists.txt
View file

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

202
env_manager/LICENSE
View file

@ -1,202 +0,0 @@
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 2022 Robossembler
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.

149
env_manager/README.md
View file

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

19
env_manager/config/config.lua
View file

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

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

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

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

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

12
env_manager/config/nodes.lua
View file

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

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

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

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

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 176 KiB

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

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

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

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

0
env_manager/include/component_manager/component_template.hpp
View file

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

21
env_manager/package.xml
View file

@ -1,21 +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>env_manager</name>
<version>0.0.0</version>
<description>env_manager</description>
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake_ros</buildtool_depend>
<depend>boost</depend>
<depend>rcl</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

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

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

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

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

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

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

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

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

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

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

60
env_manager/src/main.cpp
View file

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