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

create remove pddl action

Browse source
This commit is contained in:
Splinter1984 2022-04-01 01:36:48 +08:00
parent b5fad5156a
commit 6b7bfd7a71
33 changed files with 470 additions and 296 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -22,6 +22,16 @@ namespace component_state_monitor
return _frame_name;
}
ComponentType Component::getType() const
{
return _component_type;
}
std::vector<std::string> Component::getComponentLinks() const
{
return _component_links;
}
scene_monitor_interfaces::msg::GraspPose Component::getGraspPose(const std::string &grasp_pose_name) const
{
if (_grasp_poses.find(grasp_pose_name) == _grasp_poses.end())
@ -95,6 +105,18 @@ namespace component_state_monitor
return;
}
std::string type = input.at("type");
_component_type = type == "part"? ComponentType::Part: ComponentType::Assembly;
size_t size = input.at("include").size();
if (size > 0)
{
for (auto it=input.at("include").begin(); it != input.at("include").end(); ++it)
{
_component_links.push_back(it.value());
}
}
_placement = construct_pose(input.at("features").at("placements").at("placement"));
auto grasp_poses = input.at("features").at("grasp-poses");
@ -105,8 +127,7 @@ namespace component_state_monitor
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;
pre_pose.position.z -= 0.05;
grasp_pose.pre_grasp_pose = pre_pose;