robossembler/runtime
7
0
Fork 0
Code Issues 4 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

25
rbs_skill_servers/CMakeLists.txt
View file

@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@ -30,6 +30,16 @@ find_package(rbs_skill_interfaces REQUIRED)
find_package(rmw REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(tinyxml2_vendor REQUIRED)
find_package(TinyXML2 REQUIRED)
include_directories(SYSTEM
${TINYXML2_INCLUDE_DIR}
)
link_directories(
${TINYXML2_LIBRARY_DIRS}
)
set(deps
rclcpp
@ -44,6 +54,7 @@ set(deps
rbs_skill_interfaces
tf2_eigen
tf2_msgs
tinyxml2_vendor
)
if(BUILD_TESTING)
@ -74,9 +85,6 @@ ament_target_dependencies(pick_place_pose_loader ${deps})
rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server)
rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)
add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
ament_target_dependencies(move_to_joint_states_action_server ${deps})
@ -92,6 +100,12 @@ ament_target_dependencies(move_cartesian_path_action_server ${deps})
add_executable(add_planning_scene_object_service src/add_planning_scene_objects_service.cpp)
ament_target_dependencies(add_planning_scene_object_service ${deps})
add_library(assemble_state_server SHARED src/assemble_state_server.cpp)
target_compile_definitions(assemble_state_server
PRIVATE "ASSEMBLE_STATE_SERVER_CPP_BUILDING_DLL")
ament_target_dependencies(assemble_state_server ${deps})
target_link_libraries(assemble_state_server ${TINYXML2_LIBRARY})
rclcpp_components_register_node(assemble_state_server PLUGIN "AssembleStateServer" EXECUTABLE assemble_state_service_server)
install(
DIRECTORY include/
@ -106,6 +120,7 @@ install(
move_to_joint_states_action_server
move_cartesian_path_action_server
add_planning_scene_object_service
assemble_state_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}

245
rbs_skill_servers/src/assemble_state_server.cpp Normal file
View file

@ -0,0 +1,245 @@
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <filesystem>
#include <tinyxml2.h>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.h>
#include <rbs_skill_interfaces/srv/assemble_state.hpp>
#define ASSEMBLE_PREFIX_PARAM_NAME "assemble_prefix"
#define SERVICE_NAME "assemble_state"
#define ASSEMBLE_DIR_PARAM_NAME "assemble_dir"
#define ASSEMBLE_POSITION_OFFSET 0.5
#define ASSEMBLE_ORIENTATION_OFFSET 0.5
#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \
(package_dir) + "/models/" + (assemble_name) + "/model.sdf"
static inline geometry_msgs::msg::Pose get_pose(
const std::vector<std::string> & tokens)
{
geometry_msgs::msg::Pose p;
p.position.set__x(std::stod(tokens.at(0)));
p.position.set__y(std::stod(tokens.at(1)));
p.position.set__z(std::stod(tokens.at(2)));
p.orientation.set__x(std::stod(tokens.at(3)));
p.orientation.set__y(std::stod(tokens.at(4)));
p.orientation.set__z(std::stod(tokens.at(5)));
return p;
}
static inline geometry_msgs::msg::PoseStamped get_pose_stamped(
const std::string & pose_string)
{
std::stringstream ss(pose_string);
std::istream_iterator<std::string> begin(ss);
std::istream_iterator<std::string> end;
std::vector<std::string> tokens(begin, end);
geometry_msgs::msg::PoseStamped ps;
ps.set__pose(get_pose(tokens));
return ps;
}
static std::vector<geometry_msgs::msg::PoseStamped> get_assemble_poses(
const std::string & assemble_name, const std::string & part_name,
const std::string& package_dir, const std::string & assemble_prefix)
{
std::vector<geometry_msgs::msg::PoseStamped> result;
std::filesystem::path sdf_path = package_dir + assemble_name + ".sdf";
tinyxml2::XMLDocument doc;
doc.LoadFile(sdf_path.c_str());
auto model = doc.RootElement()->FirstChildElement("model");
auto joint = model->FirstChildElement("joint");
while (joint)
{
auto frame_id = joint->FirstChildElement("child")->GetText();
if (frame_id != part_name)
continue;
auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText());
ps.header.set__frame_id(assemble_prefix + part_name);
result.push_back(ps);
joint = joint->NextSiblingElement("joint");
}
return result;
}
class AssembleStateServer: public rclcpp::Node
{
public:
AssembleStateServer(rclcpp::NodeOptions options)
: Node(SERVICE_NAME, options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
{
assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string();
assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string();
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
service_ = create_service<rbs_skill_interfaces::srv::AssembleState>(
SERVICE_NAME, std::bind(&AssembleStateServer::callback, this,
std::placeholders::_1, std::placeholders::_2));
}
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);
bool call_status = false;
std::string assemble_name = request->assemble_name;
std::string part_name = request->part_name;
auto assemble = assembles_.find(request->assemble_name);
if (assemble == assembles_.end())
assembles_.insert(std::make_pair(request->assemble_name, Assemble {
.part=request->part_name,
.ws=request->workspace,
.state=NONE,
.poses=get_assemble_poses(assemble_name, part_name, assemble_dir_, assemble_prefix_)
}));
RCLCPP_INFO(get_logger(), "callback call with assemble name: %s", assemble_name.c_str());
switch(state)
{
case NONE:
call_status = false;
break;
case INITIALIZE:
call_status = (assembles_.at(assemble_name).state == NONE) &&
on_initialize(&assembles_.at(assemble_name));
break;
case VALIDATE:
response->validate_status = (call_status = (assembles_.at(assemble_name).state == INITIALIZE)) &&
on_validate(&assembles_.at(assemble_name));
break;
case COMPLETE:
call_status = (assembles_.at(assemble_name).state == VALIDATE) &&
on_complete(&assembles_.at(assemble_name));
if (call_status)
assembles_.erase(assemble_name);
break;
}
response->call_status = call_status;
response->assemble_name = assemble_name;
}
private:
enum AssembleReqState
{
NONE=-1,
INITIALIZE=0,
VALIDATE=1,
COMPLETE=2
};
struct Assemble
{
std::string part;
std::string ws;
AssembleReqState state;
std::vector<geometry_msgs::msg::PoseStamped> poses;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster;
};
bool on_initialize(Assemble* assemble)
{
RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s", assemble->part.c_str());
try
{
assemble->tf2_broadcaster = std::make_unique<tf2_ros::StaticTransformBroadcaster>(this);
} catch (const tf2::TransformException &ex){
RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", ex.what());
return false;
}
assemble->state = INITIALIZE;
for (const auto& it: assemble->poses)
{
geometry_msgs::msg::TransformStamped t;
t.header.frame_id = assemble->ws;
t.child_frame_id = it.header.frame_id;
auto pose = it.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;
assemble->tf2_broadcaster->sendTransform(t);
}
return true;
}
bool on_validate(Assemble* assemble)
{
RCLCPP_INFO(get_logger(), "validate assemble state for part: %s", assemble->part.c_str());
std::string frame_from = assemble_prefix_ + assemble->part;
std::string frame_to = assemble->part;
geometry_msgs::msg::TransformStamped ts;
try
{
ts = tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero);
} catch (const tf2::TransformException &ex) {
return false;
}
auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET &&
ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET &&
ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET;
auto orient_validate = ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET &&
ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET &&
ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET;
RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", pos_validate, orient_validate);
assemble->state = (pos_validate && orient_validate)? VALIDATE: INITIALIZE;
return assemble->state == VALIDATE;
}
bool on_complete(Assemble* assemble)
{
RCLCPP_INFO(get_logger(), "complete assemble state for part: %s", assemble->part.c_str());
try
{
assemble->tf2_broadcaster.reset();
assemble->tf2_broadcaster = NULL;
assemble->poses.clear();
} catch (const std::exception &ex) {
RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s", ex.what());
return false;
}
assemble->state = COMPLETE;
return true;
}
private:
std::map<std::string, Assemble> assembles_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster_;
std::mutex mt;
rclcpp::Service<rbs_skill_interfaces::srv::AssembleState>::SharedPtr service_;
std::string assemble_dir_;
std::string assemble_prefix_;
};
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(AssembleStateServer);

4
rbs_skill_servers/src/pick_place_pose_loader.cpp
View file

@ -24,7 +24,7 @@ void
GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response)
{
std::string rrr = request->object_name + "_place"; // TODO: replace with better name
std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name
try {
tf_data = tf_buffer_->lookupTransform(
"world", rrr.c_str(),
@ -107,4 +107,4 @@ GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector<double> pose)
aff.translation() = point;
aff.linear() = quat.toRotationMatrix();
return aff;
}
}