Enhance grasping task handling and behavior tree configurations
- Improved `get_grasp_pose_callback` with regex for grasp pose matching and added fallback mechanisms for `pregrasp_pose` and `postgrasp_pose`. - Introduced `_create_pose_msg` and `_create_pose_msg_from_name` helper functions for dynamic transform-to-pose conversion. - Updated `GetGraspPose.srv` to include `pregrasp_pose` and `postgrasp_pose` in the service response structure. - Enhanced TF listener integration in `assembly_config_service.py` for dynamic pose retrieval and parameter management. - Added `rbs_utils_interfaces` dependency in `CMakeLists.txt` and introduced new `GetGraspPose` node for behavior tree integration. - Replaced `MoveGripper` with `GripperCommand` to improve gripper state transitions. - Refactored `MoveToPose` and `GetWorkspace` nodes for enhanced logging and parameter handling. - Updated `workspace_movement.xml` and `grasp_object.xml` with improved actions and configurations.
This commit is contained in:
parent
2fd4f3e480
commit
5c2c67567d
9 changed files with 327 additions and 67 deletions
|
|
@ -27,6 +27,7 @@ find_package(env_manager_interfaces REQUIRED)
|
|||
find_package(rbs_utils REQUIRED)
|
||||
find_package(tf2_eigen REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(rbs_utils_interfaces REQUIRED)
|
||||
|
||||
if (NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
|
|
@ -52,6 +53,7 @@ set(dependencies
|
|||
rbs_utils
|
||||
tf2_ros
|
||||
tf2_eigen
|
||||
rbs_utils_interfaces
|
||||
)
|
||||
|
||||
include_directories(include)
|
||||
|
|
@ -59,11 +61,11 @@ include_directories(include)
|
|||
add_library(rbs_skill_move_topose_bt_action_client SHARED src/MoveToPose.cpp)
|
||||
list(APPEND plugin_libs rbs_skill_move_topose_bt_action_client)
|
||||
|
||||
add_library(rbs_skill_gripper_move_bt_action_client SHARED src/MoveGripper.cpp)
|
||||
add_library(rbs_skill_gripper_move_bt_action_client SHARED src/GripperCommand.cpp)
|
||||
list(APPEND plugin_libs rbs_skill_gripper_move_bt_action_client)
|
||||
|
||||
# add_library(rbs_skill_get_pick_place_pose_service_client SHARED src/GetPickPlacePoses.cpp)
|
||||
# list(APPEND plugin_libs rbs_skill_get_pick_place_pose_service_client)
|
||||
add_library(rbs_skill_get_pick_place_pose_service_client SHARED src/GetGraspPose.cpp)
|
||||
list(APPEND plugin_libs rbs_skill_get_pick_place_pose_service_client)
|
||||
|
||||
add_library(rbs_skill_move_joint_state SHARED src/MoveToJointStates.cpp)
|
||||
list(APPEND plugin_libs rbs_skill_move_joint_state)
|
||||
|
|
|
|||
|
|
@ -3,23 +3,43 @@
|
|||
<BehaviorTree ID="GraspObject">
|
||||
<Sequence>
|
||||
|
||||
<Script code="action:='/cartesian_move_to_pose'" />
|
||||
|
||||
<Action ID="GetWorkspace"
|
||||
object_name="{object_name}"
|
||||
service_name="{get_workspace}"
|
||||
grasp_pose="{grasp_pose}"
|
||||
pregrasp_pose="{pregrasp_pose}"
|
||||
<Script code="open_gripper_limit:='-0.014'" />
|
||||
<Script code="close_gripper_limit:='0.0'" />
|
||||
|
||||
<Script code="robot_name:='rbs_arm'" />
|
||||
<Script code="object_name:='box'" />
|
||||
|
||||
<Script code="grasp_service:='/get_grasp_pose'" />
|
||||
<Script code="move_action:='/mtp_jtc'" />
|
||||
<Script code="grpper_action:='/gripper_controller/gripper_cmd'" />
|
||||
|
||||
<Action ID="GetGraspPose"
|
||||
object_name="box"
|
||||
service_name="{grasp_service}"
|
||||
grasp_pose="{grasp_pose}"
|
||||
pregrasp_pose="{pregrasp_pose}"
|
||||
postgrasp_pose="{postgrasp_pose}" />
|
||||
|
||||
<Action ID="MoveToPose" pose="{pregrasp_pose}" robot_name="{robot_name}" action_name="{action}" />
|
||||
<Action ID="MoveToPose" pose="{grasp_pose}" robot_name="{robot_name}" action_name="{action}" />
|
||||
<Action ID="MoveToPose" pose="{postgrasp_pose}" robot_name="{robot_name}" action_name="{action}" />
|
||||
<Action ID="MoveToPose" pose="{pregrasp_pose}" duration="5.0" robot_name="{robot_name}"
|
||||
action_name="{move_action}" />
|
||||
|
||||
<Action ID="GripperCommand" open_limit="{open_gripper_limit}" close_limit="{close_gripper_limit}" state="open"
|
||||
action_name="{grpper_action}" />
|
||||
|
||||
<Action ID="MoveToPose" pose="{grasp_pose}" duration="2.0" robot_name="{robot_name}"
|
||||
action_name="{move_action}" />
|
||||
|
||||
<Action ID="GripperCommand" open_limit="{open_gripper_limit}" close_limit="{close_gripper_limit}" state="close"
|
||||
action_name="{grpper_action}" />
|
||||
|
||||
<Action ID="MoveToPose" pose="{postgrasp_pose}" duration="2.0" robot_name="{robot_name}"
|
||||
action_name="{move_action}" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<TreeNodesModel>
|
||||
<Action ID="GetWorkspace">
|
||||
<Action ID="GetGraspPose">
|
||||
<input_port name="object_name" />
|
||||
<input_port name="service_name" />
|
||||
<output_port name="grasp_pose" />
|
||||
|
|
@ -32,5 +52,12 @@
|
|||
<input_port name="robot_name" />
|
||||
<input_port name="action_name" />
|
||||
</Action>
|
||||
|
||||
<Action ID="GripperCommand">
|
||||
<input_port name="open_limit" />
|
||||
<input_port name="close_limit" />
|
||||
<input_port name="state" />
|
||||
<input_port name="action_name"/>
|
||||
</Action>
|
||||
</TreeNodesModel>
|
||||
</root>
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
<!-- <Script code="action:='/'+robot_name+'/cartesian_move_to_pose'" /> -->
|
||||
<!-- <Script code="get_workspace:='/'+robot_name+'/get_workspace'"/> -->
|
||||
|
||||
<Script code="action:='/cartesian_move_to_pose'"/>
|
||||
<Script code="action:='/mtp_jtc'"/>
|
||||
<Script code="get_workspace:='/get_workspace'"/>
|
||||
|
||||
<Action ID="GetWorkspace" type="box" service_name="{get_workspace}" workspace="{workspace}"/>
|
||||
|
|
|
|||
90
rbs_bt_executor/src/GetGraspPose.cpp
Normal file
90
rbs_bt_executor/src/GetGraspPose.cpp
Normal file
|
|
@ -0,0 +1,90 @@
|
|||
#include "behaviortree_ros2/bt_service_node.hpp"
|
||||
|
||||
#include "behaviortree_ros2/plugins.hpp"
|
||||
#include "rbs_utils_interfaces/srv/get_grasp_pose.hpp"
|
||||
#include <behaviortree_cpp/basic_types.h>
|
||||
#include <geometry_msgs/msg/detail/point__struct.hpp>
|
||||
#include <geometry_msgs/msg/detail/pose__struct.hpp>
|
||||
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
|
||||
#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
|
||||
#include <memory>
|
||||
#include <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
|
||||
#include <rclcpp/logging.hpp>
|
||||
#include <string>
|
||||
|
||||
using GetGraspPoseService = rbs_utils_interfaces::srv::GetGraspPose;
|
||||
using namespace BT;
|
||||
|
||||
class GetGraspPose : public RosServiceNode<GetGraspPoseService> {
|
||||
public:
|
||||
GetGraspPose(const std::string &name, const NodeConfig &conf,
|
||||
const RosNodeParams ¶ms)
|
||||
: RosServiceNode<GetGraspPoseService>(name, conf, params) {
|
||||
|
||||
RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
|
||||
}
|
||||
|
||||
static PortsList providedPorts() {
|
||||
return providedBasicPorts(
|
||||
{InputPort<std::string>("object_name"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("grasp_pose"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pregrasp_pose"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>(
|
||||
"postgrasp_pose")});
|
||||
}
|
||||
|
||||
bool setRequest(Request::SharedPtr &request) override {
|
||||
RCLCPP_INFO(this->logger(), "Starting send request");
|
||||
if (!getInput("object_name", request->model_name)) {
|
||||
RCLCPP_ERROR(this->node_.lock()->get_logger(),
|
||||
"Failed to get object_name from input port");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
|
||||
if (!response->ok) {
|
||||
RCLCPP_ERROR(this->node_.lock()->get_logger(),
|
||||
"Response indicates failure.");
|
||||
return NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->node_.lock()->get_logger(),
|
||||
"Response received successfully.");
|
||||
|
||||
auto logPose = [this](const std::string &pose_name,
|
||||
const geometry_msgs::msg::Pose &pose) {
|
||||
RCLCPP_INFO(this->node_.lock()->get_logger(),
|
||||
"%s:\n"
|
||||
" Position: x = %.4f, y = %.4f, z = %.4f\n"
|
||||
" Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
|
||||
pose_name.c_str(), pose.position.x, pose.position.y,
|
||||
pose.position.z, pose.orientation.x, pose.orientation.y,
|
||||
pose.orientation.z, pose.orientation.w);
|
||||
};
|
||||
|
||||
auto ppose = std::make_shared<geometry_msgs::msg::Pose>(response->grasp_pose);
|
||||
|
||||
logPose("Grasp Pose", response->grasp_pose);
|
||||
logPose("Pre-grasp Pose", response->pregrasp_pose);
|
||||
logPose("Post-grasp Pose", response->postgrasp_pose);
|
||||
|
||||
auto grasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
|
||||
auto pregrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
|
||||
auto postgrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
|
||||
|
||||
*grasp_pose = response->grasp_pose;
|
||||
*pregrasp_pose = response->pregrasp_pose;
|
||||
*postgrasp_pose = response->postgrasp_pose;
|
||||
|
||||
setOutput("grasp_pose", grasp_pose);
|
||||
setOutput("pregrasp_pose", pregrasp_pose);
|
||||
setOutput("postgrasp_pose", postgrasp_pose);
|
||||
|
||||
return NodeStatus::SUCCESS;
|
||||
}
|
||||
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
|
||||
};
|
||||
|
||||
CreateRosNodePlugin(GetGraspPose, "GetGraspPose");
|
||||
|
|
@ -1,8 +1,8 @@
|
|||
#include "behaviortree_ros2/bt_service_node.hpp"
|
||||
|
||||
#include "behaviortree_ros2/plugins.hpp"
|
||||
#include "rbs_utils_interfaces/srv/get_workspace.hpp"
|
||||
#include <behaviortree_cpp/basic_types.h>
|
||||
#include "behaviortree_ros2/plugins.hpp"
|
||||
#include <geometry_msgs/msg/detail/point__struct.hpp>
|
||||
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
|
||||
#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
|
||||
|
|
@ -14,14 +14,14 @@ using namespace BT;
|
|||
class GetWorkspace : public RosServiceNode<GetWorkspaceService> {
|
||||
public:
|
||||
GetWorkspace(const std::string &name, const NodeConfig &conf,
|
||||
const RosNodeParams ¶ms)
|
||||
const RosNodeParams ¶ms)
|
||||
: RosServiceNode<GetWorkspaceService>(name, conf, params) {}
|
||||
|
||||
static PortsList providedPorts() {
|
||||
return providedBasicPorts(
|
||||
{InputPort<std::string>("type"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>(
|
||||
"workspace")});
|
||||
return providedBasicPorts({
|
||||
InputPort<std::string>("type"),
|
||||
OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>("workspace")
|
||||
});
|
||||
}
|
||||
|
||||
bool setRequest(Request::SharedPtr &request) override {
|
||||
|
|
@ -29,36 +29,37 @@ public:
|
|||
return true;
|
||||
}
|
||||
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
|
||||
if (!response->ok) {
|
||||
return NodeStatus::FAILURE;
|
||||
}
|
||||
if (!response->ok) {
|
||||
return NodeStatus::FAILURE;
|
||||
}
|
||||
|
||||
auto workspace = response->workspace;
|
||||
auto workspace_arr = std::make_shared<geometry_msgs::msg::PoseArray>();
|
||||
auto quat = std::make_shared<geometry_msgs::msg::Quaternion>();
|
||||
auto workspace = response->workspace;
|
||||
auto workspace_arr = std::make_shared<geometry_msgs::msg::PoseArray>();
|
||||
auto quat = std::make_shared<geometry_msgs::msg::Quaternion>();
|
||||
|
||||
quat->w = 0.0;
|
||||
quat->x = 0.0;
|
||||
quat->y = 1.0;
|
||||
quat->z = 0.0;
|
||||
quat->w = 0.0;
|
||||
quat->x = 0.0;
|
||||
quat->y = 1.0;
|
||||
quat->z = 0.0;
|
||||
|
||||
workspace_arr->poses.resize(workspace.size());
|
||||
workspace_arr->poses.resize(workspace.size());
|
||||
|
||||
size_t i = 0;
|
||||
for (auto &point : workspace) {
|
||||
point.z += 0.35;
|
||||
size_t i = 0;
|
||||
for (auto& point : workspace) {
|
||||
point.z += 0.35;
|
||||
|
||||
geometry_msgs::msg::Pose pose;
|
||||
pose.position = point;
|
||||
pose.orientation = *quat;
|
||||
geometry_msgs::msg::Pose pose;
|
||||
pose.position = point;
|
||||
pose.orientation = *quat;
|
||||
|
||||
workspace_arr->poses[i++] = pose;
|
||||
}
|
||||
workspace_arr->poses[i++] = pose;
|
||||
}
|
||||
|
||||
setOutput("workspace", workspace_arr);
|
||||
return NodeStatus::SUCCESS;
|
||||
setOutput("workspace", workspace_arr);
|
||||
return NodeStatus::SUCCESS;
|
||||
}
|
||||
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
|
||||
};
|
||||
|
||||
CreateRosNodePlugin(GetWorkspace, "GetWorkspace");
|
||||
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
#include <behaviortree_ros2/plugins.hpp>
|
||||
#include <behaviortree_ros2/ros_node_params.hpp>
|
||||
#include <control_msgs/action/detail/gripper_command__struct.hpp>
|
||||
#include <rclcpp/logging.hpp>
|
||||
|
||||
using GripperCommand = control_msgs::action::GripperCommand;
|
||||
using namespace BT;
|
||||
|
|
@ -14,12 +15,6 @@ public:
|
|||
const RosNodeParams ¶ms)
|
||||
: RosActionNode<GripperCommand>(name, conf, params) {
|
||||
|
||||
auto state = getInput<std::string>("state").value();
|
||||
auto open_limit = getInput<double>("open_limit").value();
|
||||
auto close_limit = getInput<double>("close_limit").value();
|
||||
|
||||
position.open = open_limit;
|
||||
position.close = close_limit;
|
||||
}
|
||||
|
||||
static PortsList providedPorts() {
|
||||
|
|
@ -31,8 +26,24 @@ public:
|
|||
}
|
||||
|
||||
bool setGoal(RosActionNode::Goal &goal) override {
|
||||
goal.command.position = position.open;
|
||||
goal.command.max_effort = 0.0;
|
||||
|
||||
auto state = getInput<std::string>("state").value();
|
||||
auto open_limit = getInput<double>("open_limit").value();
|
||||
auto close_limit = getInput<double>("close_limit").value();
|
||||
|
||||
position.open = open_limit;
|
||||
position.close = close_limit;
|
||||
if (state == "open") {
|
||||
goal.command.position = position.open;
|
||||
} else if (state == "close") {
|
||||
goal.command.position = position.close;
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(node_.lock()->get_logger(),
|
||||
"Error: Invalid state '"
|
||||
<< state << "'. Expected 'open' or 'close'.");
|
||||
return false;
|
||||
}
|
||||
goal.command.max_effort = 1.0;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -48,8 +59,6 @@ private:
|
|||
double open = 0.008;
|
||||
double close = 0.000;
|
||||
} position;
|
||||
|
||||
std::string pose;
|
||||
};
|
||||
|
||||
CreateRosNodePlugin(GripperControl, "MoveGripper");
|
||||
CreateRosNodePlugin(GripperControl, "GripperCommand");
|
||||
|
|
@ -11,27 +11,37 @@ class MoveToPose : public RosActionNode<MoveitSendPoseAction> {
|
|||
public:
|
||||
MoveToPose(const std::string &name, const NodeConfig &conf,
|
||||
const RosNodeParams ¶ms)
|
||||
: RosActionNode<MoveitSendPoseAction>(name, conf, params) {}
|
||||
: RosActionNode<MoveitSendPoseAction>(name, conf, params) {
|
||||
|
||||
RCLCPP_INFO(this->logger(), "Starting MoveToPose");
|
||||
}
|
||||
|
||||
static BT::PortsList providedPorts() {
|
||||
return providedBasicPorts(
|
||||
{BT::InputPort<std::string>("robot_name"),
|
||||
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose")});
|
||||
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
|
||||
BT::InputPort<double>("duration")});
|
||||
}
|
||||
|
||||
bool setGoal(RosActionNode::Goal &goal) override {
|
||||
RCLCPP_INFO(this->logger(), "Starting send request");
|
||||
getInput("robot_name", goal.robot_name);
|
||||
getInput("pose", goal.target_pose);
|
||||
goal.duration = 2.0;
|
||||
getInput("pose", m_target_pose);
|
||||
getInput("duration", goal.duration);
|
||||
goal.target_pose = *m_target_pose;
|
||||
return true;
|
||||
}
|
||||
|
||||
NodeStatus onResultReceived(const WrappedResult &wr) override {
|
||||
|
||||
RCLCPP_INFO(this->logger(), "Starting get response");
|
||||
if (!wr.result->success) {
|
||||
return NodeStatus::FAILURE;
|
||||
}
|
||||
return NodeStatus::SUCCESS;
|
||||
}
|
||||
private:
|
||||
std::shared_ptr<geometry_msgs::msg::Pose> m_target_pose;
|
||||
};
|
||||
|
||||
CreateRosNodePlugin(MoveToPose, "MoveToPose");
|
||||
|
|
|
|||
|
|
@ -9,6 +9,8 @@ from rbs_utils_interfaces.msg import NamedPose, RelativeNamedPose, AssemblyConfi
|
|||
from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace
|
||||
from rbs_assets_library import get_asm_config
|
||||
from env_manager.utils import Tf2Broadcaster
|
||||
from env_manager.utils import Tf2Listener
|
||||
import re
|
||||
|
||||
|
||||
class AssemblyConfigService(Node):
|
||||
|
|
@ -17,16 +19,21 @@ class AssemblyConfigService(Node):
|
|||
|
||||
# Initialize parameters
|
||||
self.declare_parameter("assembly_config_name", "board_pick_and_place")
|
||||
config_name = self.get_parameter("assembly_config_name").get_parameter_value().string_value
|
||||
self.declare_parameter("base_link", "world")
|
||||
# self.declare_parameter("gripper_offset", 0.055)
|
||||
config_name = (
|
||||
self.get_parameter("assembly_config_name")
|
||||
.get_parameter_value()
|
||||
.string_value
|
||||
)
|
||||
if config_name:
|
||||
self.get_logger().info(f"Loading assembly config with name: {config_name}")
|
||||
asm_config_filepath = get_asm_config(config_name)
|
||||
yaml_file = os.path.join(
|
||||
asm_config_filepath
|
||||
)
|
||||
yaml_file = os.path.join(asm_config_filepath)
|
||||
self.assembly_config = parse_yaml(yaml_file)
|
||||
|
||||
tf2_broadcaster = Tf2Broadcaster(self)
|
||||
self.tf2_listner = Tf2Listener(self)
|
||||
for relative_part in self.assembly_config.relative_part:
|
||||
tf2_broadcaster.broadcast_tf(
|
||||
relative_part.relative_at,
|
||||
|
|
@ -40,10 +47,10 @@ class AssemblyConfigService(Node):
|
|||
relative_part.pose.orientation.x,
|
||||
relative_part.pose.orientation.y,
|
||||
relative_part.pose.orientation.z,
|
||||
relative_part.pose.orientation.w
|
||||
)
|
||||
relative_part.pose.orientation.w,
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
for grasp_pose in self.assembly_config.grasp_pose:
|
||||
tf2_broadcaster.broadcast_tf(
|
||||
grasp_pose.relative_at,
|
||||
|
|
@ -58,10 +65,9 @@ class AssemblyConfigService(Node):
|
|||
grasp_pose.pose.orientation.y,
|
||||
grasp_pose.pose.orientation.z,
|
||||
grasp_pose.pose.orientation.w,
|
||||
)
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
# Services
|
||||
self.workspace_service = self.create_service(
|
||||
GetWorkspace, "get_workspace", self.get_workspace_callback
|
||||
|
|
@ -77,10 +83,121 @@ class AssemblyConfigService(Node):
|
|||
return response
|
||||
|
||||
def get_grasp_pose_callback(self, request, response):
|
||||
response.grasp_pose = self.assembly_config.grasp_pose
|
||||
# Create regular expressions for grasp poses
|
||||
grasp_pose_pattern = re.compile(
|
||||
rf"^{re.escape(request.model_name)}_grasp_pose$"
|
||||
)
|
||||
pregrasp_pose_pattern = re.compile(
|
||||
rf"^{re.escape(request.model_name)}_pregrasp_pose$"
|
||||
)
|
||||
postgrasp_pose_pattern = re.compile(
|
||||
rf"^{re.escape(request.model_name)}_postgrasp_pose$"
|
||||
)
|
||||
prepostgrasp_pose_pattern = re.compile(
|
||||
rf"^{re.escape(request.model_name)}_prepostgrasp_pose$"
|
||||
)
|
||||
|
||||
# Find poses using generators
|
||||
grasp_pose_ = next(
|
||||
(
|
||||
pose
|
||||
for pose in self.assembly_config.grasp_pose
|
||||
if grasp_pose_pattern.fullmatch(pose.name)
|
||||
),
|
||||
None,
|
||||
)
|
||||
pregrasp_pose_ = next(
|
||||
(
|
||||
pose
|
||||
for pose in self.assembly_config.grasp_pose
|
||||
if pregrasp_pose_pattern.fullmatch(pose.name)
|
||||
),
|
||||
None,
|
||||
)
|
||||
postgrasp_pose_ = next(
|
||||
(
|
||||
pose
|
||||
for pose in self.assembly_config.grasp_pose
|
||||
if postgrasp_pose_pattern.fullmatch(pose.name)
|
||||
),
|
||||
None,
|
||||
)
|
||||
prepostgrasp_pose_ = next(
|
||||
(
|
||||
pose
|
||||
for pose in self.assembly_config.grasp_pose
|
||||
if prepostgrasp_pose_pattern.fullmatch(pose.name)
|
||||
),
|
||||
None,
|
||||
)
|
||||
|
||||
# If grasp_pose is not found, raise an error
|
||||
if grasp_pose_ is None:
|
||||
response.ok = False
|
||||
raise RuntimeError(f"Grasp pose not found for model {request.model_name}")
|
||||
|
||||
grasp_pose_msg = self._create_pose_msg_from_name(grasp_pose_)
|
||||
|
||||
# Create messages for pregrasp and postgrasp, considering prepostgrasp
|
||||
if prepostgrasp_pose_:
|
||||
pregrasp_pose_msg = self._create_pose_msg_from_name(prepostgrasp_pose_)
|
||||
postgrasp_pose_msg = pregrasp_pose_msg
|
||||
else:
|
||||
pregrasp_pose_msg = self._create_pose_msg_from_name(pregrasp_pose_)
|
||||
postgrasp_pose_msg = self._create_pose_msg_from_name(postgrasp_pose_)
|
||||
|
||||
# Ensure pregrasp and postgrasp have values
|
||||
if pregrasp_pose_msg is None:
|
||||
pregrasp_pose_msg = (
|
||||
Pose()
|
||||
) # Fill with an empty Pose object if pregrasp is missing
|
||||
|
||||
if postgrasp_pose_msg is None:
|
||||
postgrasp_pose_msg = (
|
||||
pregrasp_pose_msg # Use pregrasp if postgrasp is missing
|
||||
)
|
||||
|
||||
# Fill the response
|
||||
response.grasp_pose = grasp_pose_msg
|
||||
response.pregrasp_pose = pregrasp_pose_msg
|
||||
response.postgrasp_pose = postgrasp_pose_msg
|
||||
response.ok = True
|
||||
return response
|
||||
|
||||
def _create_pose_msg(self, transform):
|
||||
# Convert the transform to a Pose message
|
||||
if transform is None:
|
||||
raise RuntimeError("Pose transform not found")
|
||||
|
||||
# gripper_offset = self.get_parameter("gripper_offset").get_parameter_value().double_value
|
||||
|
||||
pose_msg = Pose()
|
||||
pose_msg.position.x = transform.translation.x
|
||||
pose_msg.position.y = -transform.translation.y
|
||||
pose_msg.position.z = transform.translation.z #+ gripper_offset
|
||||
|
||||
pose_msg.orientation.x = transform.rotation.x
|
||||
pose_msg.orientation.y = transform.rotation.y
|
||||
pose_msg.orientation.z = transform.rotation.z
|
||||
pose_msg.orientation.w = transform.rotation.w
|
||||
|
||||
return pose_msg
|
||||
|
||||
def _create_pose_msg_from_name(self, pose):
|
||||
# Convert RelativeNamedPose to Pose (stub, depends on your implementation)
|
||||
if pose is None:
|
||||
return None
|
||||
|
||||
# Assumes that pose contains data that can be used to form a Pose
|
||||
transform = self.tf2_listner.lookup_transform_sync(
|
||||
target_frame=pose.name,
|
||||
source_frame=self.get_parameter("base_link")
|
||||
.get_parameter_value()
|
||||
.string_value,
|
||||
)
|
||||
return self._create_pose_msg(transform)
|
||||
|
||||
|
||||
def parse_yaml(yaml_file):
|
||||
with open(yaml_file, "r") as file:
|
||||
data = yaml.safe_load(file)
|
||||
|
|
@ -139,6 +256,7 @@ def parse_yaml(yaml_file):
|
|||
|
||||
return assembly_config
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
|
||||
|
|
@ -150,5 +268,6 @@ def main():
|
|||
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -1,4 +1,6 @@
|
|||
string model_name
|
||||
---
|
||||
rbs_utils_interfaces/RelativeNamedPose[] grasp_pose
|
||||
geometry_msgs/Pose grasp_pose
|
||||
geometry_msgs/Pose pregrasp_pose
|
||||
geometry_msgs/Pose postgrasp_pose
|
||||
bool ok
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue