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

add graspPosesSrv

Browse source
This commit is contained in:
Splinter1984 2022-02-24 00:30:47 +08:00
parent a9105b3992
commit 718fd94d18
10 changed files with 57 additions and 50 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -22,9 +22,26 @@ namespace component_state_monitor
return _frame_name;
}
scene_monitor_interfaces::msg::GraspPose Component::getGraspPose() const
scene_monitor_interfaces::msg::GraspPose Component::getGraspPose(const std::string &grasp_pose_name) const
{
return _grasp_poses.at("grasp-pose-1");
if (_grasp_poses.find(grasp_pose_name) == _grasp_poses.end())
{
RCLCPP_WARN(rclcpp::get_logger("component"), "GraspPose with name (%s) not found", grasp_pose_name.c_str());
return scene_monitor_interfaces::msg::GraspPose();
}
return _grasp_poses.at(grasp_pose_name);
}
std::vector<scene_monitor_interfaces::msg::GraspPose> Component::getGraspPoses() const
{
std::vector<scene_monitor_interfaces::msg::GraspPose> grasp_poses;
for (auto &pose: _grasp_poses)
{
grasp_poses.push_back(pose.second);
}
return grasp_poses;
}
geometry_msgs::msg::Pose Component::getPlacementPose() const