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

Motion planner (MoveIt) and Task planner (Plansys) integration

Browse source
This commit is contained in:
Roman Andrianov 2023-04-30 11:46:52 +00:00 • committed by Igor Brylyov
parent 0a735b87c9
commit 000ddb4831
43 changed files with 1402 additions and 379 deletions
Download patch file Download diff file Expand all files Collapse all files

58
env_components/assemble_component/CMakeLists.txt
View file

@ -1,58 +0,0 @@
cmake_minimum_required(VERSION 3.5)
project(assemble_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(rbs_skill_interfaces REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(env_manager REQUIRED)
add_library(assemble_state_component SHARED
src/assemble_state_component.cpp)
target_compile_definitions(assemble_state_component
PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(assemble_state_component
"rbs_skill_interfaces"
"rclcpp"
"rclcpp_components"
"tf2_ros"
"tf2_msgs"
"geometry_msgs"
"env_manager")
rclcpp_components_register_nodes(assemble_state_component "assemble_component::AssembleStateComponent")
set(node_plugins "${node_plugins}assemble_component::AssembleStateComponent;$<TARGET_FILE:assemble_component>\n")
install(TARGETS assemble_state_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()

233
env_components/assemble_component/src/assemble_state_component.cpp
View file

@ -1,233 +0,0 @@
#include <component_manager/service_component.hpp>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_msgs/msg/tf_message.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rbs_skill_interfaces/srv/assemble_state.hpp>
namespace assemble_component
{
const std::string ASSEMBLE_PACKAGE_NAME_PARAM = "assemble_package_name";
const std::string ASSEMBLE_PREFIX_NAME = "ASSEMBLE";
const double ASSEMBLE_POSITION_OFFSET = 0.5;
const double ASSEMBLE_ORIENTATION_OFFSET = 0.5;
/* TODO: implement sdf parse for assemble_solver.
* */
struct assemble_solver
{
std::string package_name;
std::string assemble_name;
std::string part_name;
std::string workspace;
std::map<std::string, geometry_msgs::msg::PoseStamped> assemble_parts;
};
class AssembleStateComponent: public env_manager::component_manager
::ServiceComponent<rbs_skill_interfaces::srv::AssembleState>
{
public:
AssembleStateComponent(const rclcpp::NodeOptions& options)
: env_manager::component_manager
::ServiceComponent<rbs_skill_interfaces::srv::AssembleState>(options)
{
this->declare_parameter(ASSEMBLE_PACKAGE_NAME_PARAM, NULL);
param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
auto pevcallback = [this](const rclcpp::Parameter & p) {
if (ASSEMBLE_PACKAGE_NAME_PARAM == p.get_name() &&
assemble_package_name_ != p.as_string() &&
current_state_ == AssembleReqState::NONE)
assemble_package_name_ = p.as_string();
else
RCLCPP_WARN(get_logger(),
"Unable to change param while assemble process in progress.");
};
cb_handle_ = param_subscriber_->add_parameter_callback(
ASSEMBLE_PACKAGE_NAME_PARAM, pevcallback);
tf_buffer_ =
std::make_unique<tf2_ros::Buffer>(get_clock());
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
}
void callback(
std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Request> request,
std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Response> response)
{
auto state = static_cast<AssembleReqState>(request->req_kind);
auto call_status = false;
switch(state)
{
case AssembleReqState::NONE:
response->call_status = false;
return;
case AssembleReqState::INITIALIZE:
call_status = (current_state_ != AssembleReqState::NONE)? false:
on_initialize(request->assemble_name, request->part_name, request->workspace);
if (!call_status)
RCLCPP_WARN_STREAM(get_logger(), "callback: " << "initialize error.");
break;
case AssembleReqState::VALIDATE:
call_status = (current_state_ == AssembleReqState::INITIALIZE);
response->validate_status = (call_status)? on_validate(): call_status;
if (!call_status)
RCLCPP_WARN_STREAM(get_logger(), "callback: " << "validate error");
break;
case AssembleReqState::COMPLETE:
call_status = (current_state_ != AssembleReqState::VALIDATE)? false: on_complete();
if (!call_status)
RCLCPP_WARN_STREAM(get_logger(), "callback: " << "complete process error.");
break;
case AssembleReqState::CANCEL:
call_status = (current_state_ == AssembleReqState::NONE)? false: on_cancel();
if (!call_status)
RCLCPP_WARN_STREAM(get_logger(), "callback: " << "complete process error.");
break;
}
response->call_status = call_status;
}
bool on_initialize(
const std::string &assemble_name,
const std::string &part_name,
const std::string &workspace)
{
if (current_state_ != AssembleReqState::NONE)
{
RCLCPP_WARN_STREAM(get_logger(), "on_initialize: " << "assemble in progress");
return false;
}
try {
mt.lock();
assemble_ = (assemble_solver){
.package_name=assemble_package_name_,
.assemble_name=assemble_name,
.part_name=part_name,
.workspace=workspace};
mt.unlock();
} catch (std::exception &ex) {
RCLCPP_ERROR_STREAM(get_logger(), "on_initialize: " << ex.what());
return false;
}
current_state_ = AssembleReqState::INITIALIZE;
tf_broadcaster_ =
std::make_unique<tf2_ros::StaticTransformBroadcaster>(this);
send_transform(assemble_);
return true;
}
bool on_validate()
{
std::string frame_from = ASSEMBLE_PREFIX_NAME + assemble_.part_name;
std::string frame_to = assemble_.part_name;
geometry_msgs::msg::TransformStamped t;
try {
t = tf_buffer_->lookupTransform(
frame_to, frame_from, tf2::TimePointZero);
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN_STREAM(get_logger(),
"Could not transform "
<< frame_to << " to " << frame_from
<< " " << ex.what());
return false;
}
auto pos_validate =
t.transform.translation.x < ASSEMBLE_POSITION_OFFSET &&
t.transform.translation.y < ASSEMBLE_POSITION_OFFSET &&
t.transform.translation.z < ASSEMBLE_POSITION_OFFSET;
auto orient_validate =
t.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET &&
t.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET &&
t.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET &&
t.transform.rotation.w < ASSEMBLE_ORIENTATION_OFFSET;
return pos_validate && orient_validate;
}
bool on_complete()
{
try {
tf_broadcaster_.reset();
} catch (const std::exception &ex) {
RCLCPP_ERROR_STREAM(get_logger(), "on_complete: " << ex.what());
return false;
}
current_state_ = AssembleReqState::NONE;
assemble_ = {};
return true;
}
bool on_cancel()
{
return on_complete();
}
protected:
enum AssembleReqState
{
NONE = -1,
INITIALIZE = 0,
VALIDATE = 1,
COMPLETE = 2,
CANCEL = 3
};
private:
void send_transform(const assemble_solver &solver)
{
if (solver.assemble_parts.empty())
{
RCLCPP_WARN_STREAM(
get_logger(), "send_transform: " << "solver parts are empty");
return;
}
for (const auto& pt: solver.assemble_parts)
{
geometry_msgs::msg::TransformStamped t;
t.header.frame_id = solver.workspace;
t.child_frame_id = pt.first.c_str();
auto pose = pt.second.pose;
t.transform.translation.x = pose.position.x;
t.transform.translation.y = pose.position.y;
t.transform.translation.z = pose.position.z;
t.transform.rotation.x = pose.orientation.x;
t.transform.rotation.y = pose.orientation.y;
t.transform.rotation.z = pose.orientation.z;
t.transform.rotation.w = pose.orientation.w;
tf_broadcaster_->sendTransform(t);
}
}
std::string assemble_package_name_;
AssembleReqState current_state_;
std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf_broadcaster_;
assemble_solver assemble_;
std::mutex mt;
};
} // namespace assemble_component
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(assemble_component::AssembleStateComponent)

16
env_components/component_interfaces/CMakeLists.txt Normal file
View file

@ -0,0 +1,16 @@
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 Normal file
View file

@ -0,0 +1,20 @@
<?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 Normal file
View file

@ -0,0 +1,10 @@
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 Normal file
View file

@ -0,0 +1,118 @@
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()

12
env_components/assemble_component/package.xml → env_components/scene_component/package.xml
View file

@ -1,7 +1,7 @@
<?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>assemble_component</name>
<name>scene_component</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
@ -10,13 +10,17 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>pkg-config</buildtool_depend>
<depend>rbs_skill_interfaces</depend>
<depend>image_transport</depend>
<depend>ros_gz_bridge</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>env_manager</depend>
<depend>tf2_msgs</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>

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

@ -0,0 +1,109 @@
#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 Normal file
View file

@ -0,0 +1,72 @@
#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 Normal file
View file

@ -0,0 +1,115 @@
#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)