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

add scene_monitor services to robossebler node

Browse source
This commit is contained in:
Splinter1984 2022-03-29 02:23:01 +08:00
parent 4ed85d9811
commit b5fad5156a
47 changed files with 575 additions and 1023 deletions
Download patch file Download diff file Expand all files Collapse all files

13
robossembler_scene_monitor/scene_monitor/src/component.cpp
View file

@ -95,15 +95,22 @@ namespace component_state_monitor
return;
}
_placement = construct_pose(input.at("placement"));
_placement = construct_pose(input.at("features").at("placements").at("placement"));
auto grasp_poses = input.at("features").at("grasp-poses");
for (auto it = grasp_poses.begin(); it != grasp_poses.end(); ++it)
{
scene_monitor_interfaces::msg::GraspPose grasp_pose;
grasp_pose.label = it.key();
grasp_pose.placement_pose = construct_pose(it.value().at("placement"));
grasp_pose.distance = it.value().at("distance");
grasp_pose.grasp_pose = construct_pose(it.value().at("placement"));
geometry_msgs::msg::Pose pre_pose;
pre_pose.position.z = 0.12;
//pre_pose.orientation = grasp_pose.grasp_pose.orientation;
grasp_pose.pre_grasp_pose = pre_pose;
grasp_pose.gap_distance = it.value().at("distance");
_grasp_poses.insert({grasp_pose.label, grasp_pose});
}