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

add skill launch, refactor pick-place pose loader

Browse source
This commit is contained in:
Ilya Uraev 2023-11-30 13:47:33 +03:00
parent 034e172f62
commit d72c06efea
14 changed files with 1032 additions and 191 deletions
Download patch file Download diff file Expand all files Collapse all files

33
rbs_bt_executor/src/GetPickPlacePoses.cpp
View file

@ -9,9 +9,7 @@ using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses;
class GetPickPlacePose : public BtService<GetPickPlacePoseClient> {
public:
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
: BtService<GetPickPlacePoseClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
: BtService<GetPickPlacePoseClient>(name, config) {}
GetPickPlacePoseClient::Request::SharedPtr populate_request() override {
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
@ -31,24 +29,23 @@ public:
BT::NodeStatus handle_response(
GetPickPlacePoseClient::Response::SharedPtr response) override {
RCLCPP_INFO(_node->get_logger(),
"\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
"\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
"orientation.z: %f \n\t orientation.w: %f",
response->grasp[2].position.x, response->grasp[2].position.y,
response->grasp[2].position.z, response->grasp[2].orientation.x,
response->grasp[2].orientation.y,
response->grasp[2].orientation.z,
response->grasp[2].orientation.w);
// RCLCPP_INFO(_node->get_logger(),
// "\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
// "\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
// "orientation.z: %f \n\t orientation.w: %f",
// response->grasp[1].position.x, response->grasp[1].position.y,
// response->grasp[1].position.z, response->grasp[1].orientation.x,
// response->grasp[1].orientation.y,
// response->grasp[1].orientation.z,
// response->grasp[1].orientation.w);
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[3]);
setOutput<geometry_msgs::msg::Pose>("PostPostGraspPose",
response->grasp[2]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[2]);
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[2]);
return BT::NodeStatus::SUCCESS;
}
@ -60,8 +57,8 @@ public:
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
OutputPort<geometry_msgs::msg::Pose>(
"PostPostGraspPose"), // TODO: change to std::vector<>
// TODO: change to std::vector<>
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),