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

v0.1

Browse source
This commit is contained in:
Splinter1984 2023-05-16 23:10:19 +02:00
parent 000ddb4831
commit 61878375aa
8 changed files with 303 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

11
rbs_simulation/launch/rbs_simulation.launch.py
View file

@ -434,6 +434,16 @@ def generate_launch_description():
{'assemble_dir': '/home/splinter1984/tmp_ws/src/robossembler-ros2/rbs_task_planner/example/sdf_models/'}
]
)
moveit_planning_scene_init = Node(
package="rbs_skill_servers",
executable="moveit_update_planning_scene_service_server",
output="screen",
parameters=[
{'init_scene': world_config_file},
{'models_paths': os.environ['IGN_GAZEBO_RESOURCE_PATH']}
]
)
# add_planning_scene_object = Node(
# package="rbs_skill_servers",
@ -463,6 +473,7 @@ def generate_launch_description():
move_joint_state_action_server,
grasp_pose_loader,
assemble_state,
moveit_planning_scene_init,
#add_planning_scene_object
]

34
rbs_simulation/worlds/mir.sdf
View file

@ -103,7 +103,7 @@
<geometry>
<mesh>
<scale>0.35 0.35 0.35</scale>
<uri>model://rack/model/rack.dae</uri>
<uri>package://rbs_simulation/rack/model/rack.dae</uri>
</mesh>
</geometry>
</visual>
@ -111,7 +111,7 @@
<geometry>
<mesh>
<scale>0.35 0.35 0.35</scale>
<uri>model://rack/model/rack.stl</uri>
<uri>package://rbs_simulation/rack/model/rack.stl</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
@ -141,7 +141,7 @@
<geometry>
<!-- <mesh>
<scale>1 1 1</scale>
<uri>model://table/model/table.dae</uri>
<uri>package://rbs_simulation/table/model/table.dae</uri>
</mesh> -->
<box>
<size>0.8 0.8 0.4</size>
@ -157,7 +157,7 @@
<geometry>
<!-- <mesh>
<scale>1 1 1</scale>
<uri>model://table/model/table.stl</uri>
<uri>package://rbs_simulation/table/model/table.stl</uri>
</mesh> -->
<box>
<size>0.8 0.8 0.4</size>
@ -183,62 +183,62 @@
</link>
</model>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>box1</name>
<pose>0.25 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>box2</name>
<pose>0 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>box3</name>
<pose>-0.25 0.65 0.6515 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>box4</name>
<pose>0.25 0.65 0.3015 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>box5</name>
<pose>0 0.65 0.3015 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>box6</name>
<pose>-0.25 0.65 0.3015 0 0 0</pose>
</include>
</include>
<!-- <include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>pyramid1</name>
<pose>-0.9000 -0.05 0.5250 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>pyramid2</name>
<pose>-0.9000 0.0 0.5250 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>pyramid3</name>
<pose>-0.9000 0.05 0.5250 0 0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>pyramid4</name>
<pose>-0.9 -0.0243 0.5750 0 -0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>pyramid5</name>
<pose>-0.9 0.0260 0.5750 0 -0 0</pose>
</include>
<include>
<uri>model://box1</uri>
<uri>package://rbs_simulation/box1</uri>
<name>pyramid6</name>
<pose>-0.9 0 0.6250 0 0 0</pose>
</include> -->

4
rbs_skill_interfaces/CMakeLists.txt
View file

@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(shape_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
@ -26,7 +27,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/AssembleState.srv"
"srv/GetPickPlacePoses.srv"
"srv/AddPlanningSceneObject.srv"
DEPENDENCIES std_msgs geometry_msgs moveit_msgs
"srv/PlanningSceneObjectState.srv"
DEPENDENCIES std_msgs geometry_msgs moveit_msgs shape_msgs
)

1
rbs_skill_interfaces/package.xml
View file

@ -14,6 +14,7 @@
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<depend>shape_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

15
rbs_skill_interfaces/srv/PlanningSceneObjectState.srv Normal file
View file

@ -0,0 +1,15 @@
# ADD - add new object to planning scene
uint8 ADD=0
# REMOVE - remove object from planning scene
uint8 REMOVE=1
# UPDATE - update object positions or geometry
uint8 UPDATE=2
string frame_name
shape_msgs/Mesh mesh
geometry_msgs/PoseStamped pose
uint8 req_kind
---
bool call_status
string error_msg

44
rbs_skill_servers/CMakeLists.txt
View file

@ -33,6 +33,38 @@ find_package(tf2_msgs REQUIRED)
find_package(tinyxml2_vendor REQUIRED)
find_package(TinyXML2 REQUIRED)
# Default to Fortress
set(SDF_VER 12)
# If the user didn't specify a GZ distribution, pick the one matching the ROS distribution according to REP 2000
if(NOT DEFINED ENV{GZ_VERSION} AND DEFINED ENV{ROS_DISTRO})
if("$ENV{ROS_DISTRO}" STREQUAL "humble")
set(ENV{GZ_VERSION} "fortress")
endif()
endif()
# Find libsdformat matching the picked GZ distribution
if("$ENV{GZ_VERSION}" STREQUAL "fortress")
find_package(sdformat12 REQUIRED)
set(SDF_VER ${sdformat12_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Fortress (libSDFormat 12)")
elseif("$ENV{GZ_VERSION}" STREQUAL "garden")
find_package(sdformat13 REQUIRED)
set(SDF_VER ${sdformat13_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Garden (libSDFormat 13)")
# No GZ distribution specified, find any version of libsdformat we can
else()
foreach(major RANGE 13 9)
find_package(sdformat${major} QUIET)
if(sdformat${major}_FOUND)
# Next `find_package` call will be a noop
set(SDF_VER ${major})
message(STATUS "Compiling against libSDFormat ${major}")
break()
endif()
endforeach()
endif()
include_directories(SYSTEM
${TINYXML2_INCLUDE_DIR}
)
@ -55,6 +87,8 @@ set(deps
tf2_eigen
tf2_msgs
tinyxml2_vendor
geometric_shapes
sdformat${SDF_VER}
)
if(BUILD_TESTING)
@ -107,6 +141,15 @@ 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)
add_library(moveit_update_planning_scene_server SHARED src/moveit_update_planning_scene.cpp)
target_compile_definitions(moveit_update_planning_scene_server
PRIVATE "MOVEIT_UPDATE_PLANNING_SCENE_SERVER_CPP_BUILDING_DLL")
ament_target_dependencies(moveit_update_planning_scene_server ${deps})
target_link_libraries(moveit_update_planning_scene_server ${TINYXML2_LIBRARY})
rclcpp_components_register_node(moveit_update_planning_scene_server PLUGIN "UpdatePlanningSceneServer" EXECUTABLE moveit_update_planning_scene_service_server)
install(
DIRECTORY include/
DESTINATION include
@ -121,6 +164,7 @@ install(
move_cartesian_path_action_server
add_planning_scene_object_service
assemble_state_server
moveit_update_planning_scene_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}

1
rbs_skill_servers/package.xml
View file

@ -17,6 +17,7 @@
<depend>tf2_ros</depend>
<depend>rclcpp_action</depend>
<depend>geometry_msgs</depend>
<depend>geometric_shapes</depend>
<depend>action_msgs</depend>
<depend>rbs_skill_interfaces</depend>
<depend>tf2_eigen</depend>

211
rbs_skill_servers/src/moveit_update_planning_scene.cpp Normal file
View file

@ -0,0 +1,211 @@
#include <iostream>
#include <vector>
#include <filesystem>
#include <tinyxml2.h>
#include <geometric_shapes/mesh_operations.h>
#include <geometric_shapes/shape_operations.h>
#include <sdf/sdf.hh>
#include <sdf/World.hh>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "rbs_skill_interfaces/srv/planning_scene_object_state.hpp"
#define SERVICE_NAME "update_planning_scene"
#define INIT_SCENE_PARAM_NAME "init_scene"
#define CUSTOM_MODEL_PATH_PARAM_NAME "models_paths"
static geometry_msgs::msg::Pose pose_from_pose3d(const gz::math::Pose3d &pose)
{
geometry_msgs::msg::Pose result;
auto position = pose.Pos();
result.position.set__x(position.X());
result.position.set__y(position.Y());
result.position.set__z(position.Z());
auto orientation = pose.Rot();
result.orientation.set__x(orientation.X());
result.orientation.set__y(orientation.Y());
result.orientation.set__z(orientation.Z());
result.orientation.set__w(orientation.W());
return result;
}
static moveit_msgs::msg::CollisionObject get_object(const sdf::Model *model)
{
moveit_msgs::msg::CollisionObject obj;
obj.header.frame_id = "world";
obj.id = model->Name();
obj.pose = pose_from_pose3d(model->RawPose());
size_t link_count = model->LinkCount();
for (size_t i = 0; i < link_count; ++i)
{
auto link = model->LinkByIndex(i);
auto collision = link->CollisionByIndex(0); // NOTE: ok for right now.
auto link_pose = pose_from_pose3d(link->RawPose());
auto geometry = collision->Geom();
switch(geometry->Type())
{
case sdf::GeometryType::MESH:
{
shapes::Mesh *m = shapes::createMeshFromResource(geometry->MeshShape()->Uri());
auto scale = geometry->MeshShape()->Scale().X();
m->scale(scale);
Eigen::Vector3d centroid = {0, 0, 0};
for (size_t i = 0; i < 3 * m->vertex_count; i += 3)
{
const auto x = m->vertices[i];
const auto y = m->vertices[i+1];
const auto z = m->vertices[i+2];
centroid.x() += x*(1-scale);
centroid.y() += y*(1-scale);
centroid.z() += z*(1-scale);
}
centroid = centroid / m->vertex_count;
for (size_t i = 0; i < 3 * m->vertex_count; i += 3) {
m->vertices[i] -= centroid.x();
m->vertices[i + 1] -= centroid.y();
m->vertices[i + 2] -= centroid.z();
}
shape_msgs::msg::Mesh mesh;
shapes::ShapeMsg m_msg;
shapes::constructMsgFromShape(m, m_msg);
mesh = boost::get<shape_msgs::msg::Mesh>(m_msg);
obj.meshes.push_back(mesh);
obj.mesh_poses.push_back(link_pose);
break;
} case sdf::GeometryType::BOX: {
auto sdf_box = geometry->BoxShape();
shape_msgs::msg::SolidPrimitive box;
box.type = shape_msgs::msg::SolidPrimitive::BOX;
auto sdf_box_size = sdf_box->Size();
box.dimensions.push_back(sdf_box_size.X());
box.dimensions.push_back(sdf_box_size.Y());
box.dimensions.push_back(sdf_box_size.Z());
obj.primitives.push_back(box);
obj.primitive_poses.push_back(link_pose);
break;
}
case sdf::GeometryType::EMPTY:
case sdf::GeometryType::CYLINDER:
case sdf::GeometryType::PLANE:
case sdf::GeometryType::SPHERE:
case sdf::GeometryType::HEIGHTMAP:
case sdf::GeometryType::CAPSULE:
case sdf::GeometryType::ELLIPSOID:
case sdf::GeometryType::POLYLINE:
break; //NOTE: sorry not now
}
}
return obj;
}
static std::vector<moveit_msgs::msg::CollisionObject> get_objects(const sdf::World *world)
{
std::vector<moveit_msgs::msg::CollisionObject> result;
auto models_count = world->ModelCount();
for (size_t i = 0; i < models_count; ++i)
{
try{
auto model = world->ModelByIndex(i);
result.push_back(get_object(model));
} catch (std::exception &ex){
std::cerr << ex.what() << std::endl;
}
}
return result;
}
class UpdatePlanningSceneServer: public rclcpp::Node
{
public:
UpdatePlanningSceneServer(rclcpp::NodeOptions options)
: Node("update_planning_scene_node", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
{
std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string();
std::string model_resources = get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string();
if (!scene.empty())
{
RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str());
init_scene(scene, model_resources);
}
service_ = create_service<rbs_skill_interfaces::srv::PlanningSceneObjectState>(
SERVICE_NAME, std::bind(&UpdatePlanningSceneServer::callback, this,
std::placeholders::_1, std::placeholders::_2));
}
bool init_scene(const std::string &init_scene, const std::string &model_resources)
{
sdf::Root root;
sdf::ParserConfig config;
config.AddURIPath("package://", model_resources);
sdf::Errors errors = root.Load(init_scene, config);
if (!errors.empty())
{
for (auto const &e: errors)
RCLCPP_ERROR(get_logger(), "%s", e.Message().c_str());
return false;
}
auto world = root.WorldByIndex(0);
auto objects = get_objects(world);
//NOTE: as simple as possible!
planning_scene_.applyCollisionObjects(objects);
return true;
}
void callback(
std::shared_ptr<rbs_skill_interfaces::srv::PlanningSceneObjectState::Request> request,
std::shared_ptr<rbs_skill_interfaces::srv::PlanningSceneObjectState::Response> response)
{
auto state = static_cast<UpdatePlanningSceneRequestState>(request->req_kind);
moveit_msgs::msg::CollisionObject obj;
obj.id = request->frame_name;
switch(state)
{
case ADD:
case UPDATE:
obj.meshes.resize(1);
obj.mesh_poses.resize(1);
obj.operation = state == ADD?
moveit_msgs::msg::CollisionObject::ADD:
moveit_msgs::msg::CollisionObject::MOVE;
obj.meshes.at(0) = std::move(request->mesh);
obj.mesh_poses.at(0) = std::move(request->pose.pose);
break;
case REMOVE:
obj.operation = moveit_msgs::msg::CollisionObject::REMOVE;
break;
}
response->call_status = planning_scene_.applyCollisionObject(obj);
}
private:
enum UpdatePlanningSceneRequestState
{
ADD=0,
REMOVE=1,
UPDATE=2
};
rclcpp::Service<rbs_skill_interfaces::srv::PlanningSceneObjectState>::SharedPtr service_;
moveit::planning_interface::PlanningSceneInterface planning_scene_;
};
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(UpdatePlanningSceneServer);