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

fix building errors

Browse source
This commit is contained in:
Ilya Uraev 2023-07-31 10:25:09 +03:00
parent 75355417ff
commit 99452210b8
4 changed files with 36 additions and 69 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rbs.repos
View file

@ -20,6 +20,6 @@ repositories:
url: https://github.com/solid-sinusoid/ros2_robotiq_gripper.git
version: dev
cartesian_controllers:
type: gut
type: git
url: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers.git
version: ros2

15
rbs_simulation/launch/rbs_simulation.launch.py
View file

@ -475,6 +475,16 @@ def generate_launch_description():
]
)
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",
# executable="add_planning_scene_object_service",
@ -535,11 +545,6 @@ def generate_launch_description():
grasp_pose_loader,
assemble_state,
moveit_planning_scene_init,
#bridge,
cartesian_motion_controller_spawner,
motion_control_handle_spawner,
cartesian_compliance_controller_spawner,
#pc_filter
#add_planning_scene_object
]

32
rbs_simulation/worlds/mir.sdf
View file

@ -124,7 +124,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>
@ -132,7 +132,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>
@ -162,7 +162,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>
@ -178,7 +178,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>
@ -204,62 +204,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>
<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> -->

56
rbs_skill_servers/src/moveit_update_planning_scene.cpp
View file

@ -37,26 +37,7 @@ static geometry_msgs::msg::Pose pose_from_pose3d(const ignition::math::Pose3d &p
return result;
}
static std::string get_correct_mesh_path(
const std::string& uri, const std::vector<std::string> &resources)
{
std::string result = "";
std::regex reg(R"((?:model|package)(?:\:\/)(.*))");
std::smatch m;
if (std::regex_match(uri, m, reg))
{
std::string rel_path = m[1];
std::for_each(resources.begin(), resources.end(),
[&result, &rel_path](const std::string& res){
if (result.empty())
result = std::filesystem::exists(res + rel_path)? std::string(res + rel_path): result;
});
}
return "file://" + result;
}
static moveit_msgs::msg::CollisionObject
get_object(const sdf::Model *model, const std::vector<std::string> &resources)
static moveit_msgs::msg::CollisionObject get_object(const sdf::Model *model)
{
moveit_msgs::msg::CollisionObject obj;
obj.header.frame_id = "world";
@ -66,16 +47,14 @@ get_object(const sdf::Model *model, const std::vector<std::string> &resources)
for (size_t i = 0; i < link_count; ++i)
{
auto link = model->LinkByIndex(i);
auto collision = link->CollisionByIndex(0);
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:
{
auto path = get_correct_mesh_path(geometry->MeshShape()->Uri(), resources);
shapes::Mesh *m = shapes::createMeshFromResource(
!path.empty()? path: geometry->MeshShape()->Uri());
shapes::Mesh *m = shapes::createMeshFromResource(geometry->MeshShape()->Uri());
auto scale = geometry->MeshShape()->Scale().X();
m->scale(scale);
@ -117,40 +96,23 @@ get_object(const sdf::Model *model, const std::vector<std::string> &resources)
obj.primitive_poses.push_back(link_pose);
break;
}
case sdf::GeometryType::PLANE:
{
auto sdf_plane = geometry->PlaneShape();
shape_msgs::msg::Plane plane;
auto normal = sdf_plane->Normal();
plane.coef[0] = normal.X();
plane.coef[1] = normal.Y();
plane.coef[2] = normal.Z();
obj.planes.push_back(plane);
obj.plane_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;
break; //NOTE: sorry not now
}
}
return obj;
}
static std::vector<moveit_msgs::msg::CollisionObject>
get_objects(const sdf::World *world, const std::string &model_resources)
static std::vector<moveit_msgs::msg::CollisionObject> get_objects(const sdf::World *world)
{
std::vector<std::string> resources;
std::regex reg("\\:+");
std::sregex_token_iterator begin(
model_resources.begin(), model_resources.end(), reg, -1), end;
std::copy(++begin, end, std::back_inserter(resources));
std::vector<moveit_msgs::msg::CollisionObject> result;
auto models_count = world->ModelCount();
@ -158,7 +120,7 @@ get_objects(const sdf::World *world, const std::string &model_resources)
{
try{
auto model = world->ModelByIndex(i);
result.push_back(get_object(model, resources));
result.push_back(get_object(model));
} catch (std::exception &ex){
std::cerr << ex.what() << std::endl;
}
@ -191,7 +153,6 @@ public:
sdf::Root root;
sdf::ParserConfig config;
config.AddURIPath("package://", model_resources);
config.AddURIPath("model://", model_resources);
sdf::Errors errors = root.Load(init_scene, config);
if (!errors.empty())
{
@ -200,7 +161,8 @@ public:
return false;
}
auto world = root.WorldByIndex(0);
auto objects = get_objects(world, model_resources);
auto objects = get_objects(world);
//NOTE: as simple as possible!
planning_scene_.applyCollisionObjects(objects);
return true;
}