i use the moveit servo script to control the xarm lite 6 with the xbox controller. so far everything works.
now i would like to use the A-button of the Xbox controller to switch the vacuum gripper on/off. i have adapted the script so far, but the function does not work properly.
what do i have to do to toggle the vacuum gripper with the A-button of the xbox controller?
(i use the "Xbox One Elite 2" Controller (connected via USB) )
in which files i have to change the code?
i changed:
/dev_ws/src/xarm_ros2//xarm_api/config/xarm_params.yaml
-> debug: true to enable all services
include "rclcpp/rclcpp.hpp" // ->>>>>>>>>>>>>>> NEU für vaccum gripper function
include "std_srvs/srv/set_bool.hpp" // ->>>>>>>>>>>>>>> NEU für vaccum gripper function
namespace xarm_moveit_servo
{
// für verschiedene Joystick-Typen
enum JOYSTICK_TYPE
{
JOYSTICK_XBOX360_WIRED = 1, // Xbox 360 mit Kabel
JOYSTICK_XBOX360_WIRELESS = 2, // Xbox 360 kabellos
JOYSTICK_XBOX_ONE_ELITE2 = 3 // Xbox One Elite 2 Controller (neu hinzugefügt)
};
// für die Achsen des kabellosen Xbox 360 Controllers
enum XBOX360_WIRELESS_CONTROLLER_AXIS
{
XBOX360_WIRELESS_LEFT_STICK_LR = 0,
XBOX360_WIRELESS_LEFT_STICK_FB = 1,
XBOX360_WIRELESS_RIGHT_STICK_LR = 2,
XBOX360_WIRELESS_RIGHT_STICK_FB = 3,
XBOX360_WIRELESS_LEFT_TRIGGER = 4,
XBOX360_WIRELESS_RIGHT_TRIGGER = 5,
XBOX360_WIRELESS_CROSS_KEY_LR = 6,
XBOX360_WIRELESS_CROSS_KEY_FB = 7
};
// für die Achsen des Xbox One Elite 2 Controllers
enum XBOX_ONE_ELITE2_CONTROLLER_AXIS
{
XBOX_ONE_ELITE2_LEFT_STICK_LR = 0,
XBOX_ONE_ELITE2_LEFT_STICK_FB = 1,
XBOX_ONE_ELITE2_LEFT_TRIGGER = 2,
XBOX_ONE_ELITE2_RIGHT_STICK_LR = 3,
XBOX_ONE_ELITE2_RIGHT_STICK_FB = 4,
XBOX_ONE_ELITE2_RIGHT_TRIGGER = 5,
XBOX_ONE_ELITE2_CROSS_KEY_LR = 6,
XBOX_ONE_ELITE2_CROSS_KEY_FB = 7
};
// für die Buttons des Xbox 360 Controllers
enum XBOX360_CONTROLLER_BUTTON
{
XBOX360_BTN_A = 0, // A Button
XBOX360_BTN_B = 1, // B Button
XBOX360_BTN_X = 2, // X Button
XBOX360_BTN_Y = 3, // Y Button
XBOX360_BTN_LB = 4, // Left Bumper
XBOX360_BTN_RB = 5, // Right Bumper
XBOX360_BTN_BACK = 6, // Back Button
XBOX360_BTN_START = 7, // Start Button
XBOX360_BTN_POWER = 8, // Power Button
XBOX360_BTN_STICK_LEFT = 9, // Linker Stick gedrückt
XBOX360_BTN_STICK_RIGHT = 10 // Rechter Stick gedrückt
};
// für die Buttons des Xbox One Elite 2 Controllers
enum XBOX_ONE_ELITE2_CONTROLLER_BUTTON
{
XBOX_ONE_ELITE2_BTN_A = 0,
XBOX_ONE_ELITE2_BTN_B = 1,
XBOX_ONE_ELITE2_BTN_X = 2,
XBOX_ONE_ELITE2_BTN_Y = 3,
XBOX_ONE_ELITE2_BTN_LB = 4,
XBOX_ONE_ELITE2_BTN_RB = 5,
XBOX_ONE_ELITE2_BTN_BACK = 6,
XBOX_ONE_ELITE2_BTN_START = 7,
XBOX_ONE_ELITE2_BTN_POWER = 8,
XBOX_ONE_ELITE2_BTN_STICK_LEFT = 9,
XBOX_ONE_ELITE2_BTN_STICK_RIGHT = 10
};
// Konstruktor für den JoyToServoPub-Node
JoyToServoPub::JoyToServoPub(const rclcpp::NodeOptions& options)
: Node("joy_to_twist_publisher", options), // Initialisiert den Node mit Namen "joy_to_twistpublisher"
dof(7), // Grad der Freiheit des Roboters
ros_queuesize(10), // ROS-Nachrichtenwarteschlange
joysticktype(JOYSTICK_XBOX360_WIRED), // Standard-Joystick ist Xbox 360 USB
initializedstatus(10), // Initialisierungsstatus
joytopic("/joy"), // ROS-Topic für Joystick-Nachrichten
cartesian_command_intopic("/servo_server/delta_twist_cmds"), // Topic für kartesische Befehle
joint_command_intopic("/servo_server/delta_joint_cmds"), // Topic für Gelenk-Befehle
robot_link_commandframe("link_base"), // Basislink des Roboters
ee_framename("link_eef"), // Endeffektorrahmen
planningframe("link_base"), // Planungsrahmen (initial auf Basislink)
greiferstatus(false) // Initialisierung des Greifer-Status
{
// Initialisieren der Parameter (falls nicht vorhanden, werden Standardwerte gesetzt)
_declare_or_getparam(dof, "dof", dof_);
_declare_or_get_param(ros_queuesize, "ros_queue_size", ros_queuesize);
_declare_or_get_param(joysticktype, "joystick_type", joysticktype);
_declare_or_get_param(joytopic, "joy_topic", joytopic);
_declare_or_get_param(cartesian_command_intopic, "moveit_servo.cartesian_command_in_topic", cartesian_command_intopic);
_declare_or_get_param(joint_command_intopic, "moveit_servo.joint_command_in_topic", joint_command_intopic);
_declare_or_get_param(robot_link_commandframe, "moveit_servo.robot_link_command_frame", robot_link_commandframe);
_declare_or_get_param(ee_framename, "moveit_servo.ee_frame_name", ee_framename);
_declare_or_get_param(planningframe, "moveit_servo.planning_frame", planningframe);
// Passe die Themennamen an, falls sie mit "~/" beginnen
if (cartesian_command_in_topic_.rfind("~/", 0) == 0) {
cartesian_command_in_topic_ = "/servo_server/" + cartesian_command_in_topic_.substr(2, cartesian_command_in_topic_.length());
}
if (joint_command_in_topic_.rfind("~/", 0) == 0) {
joint_command_in_topic_ = "/servo_server/" + joint_command_in_topic_.substr(2, joint_command_in_topic_.length());
}
// Einrichten von Publisher und Subscriber
joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(joy_topic_, ros_queue_size_, std::bind(&JoyToServoPub::_joy_callback, this, std::placeholders::_1));
twist_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(cartesian_command_in_topic_, ros_queue_size_);
joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(joint_command_in_topic_, ros_queue_size_);
// Einrichten eines Service-Clients zum Starten des ServoServers
servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_server/start_servo");
servo_start_client_->wait_for_service(std::chrono::seconds(1)); // Warte auf den Service
servo_start_client_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>()); // Asynchroner Request, um den Servo zu starten
// NEU: Initialisierung des Service-Clients für den Vakuumgreifer
gripper_client_ = this->create_client<std_srvs::srv::SetBool>("/open_lite6_gripper");
}
// Hilfsfunktion zum Abrufen von Parametern
template
void JoyToServoPub::_declare_or_get_param(T& output_value, const std::string& param_name, const T default_value)
{
try
{
if (this->has_parameter(param_name))
{
// Wenn der Parameter existiert, lese ihn aus
this->get_parameter(param_name, output_value);
}
else
{
// sonst: deklariere den Parameter mit einem Standardwert
output_value = this->declare_parameter(param_name, default_value);
}
}
catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
{
RCLCPP_WARN_STREAM(this->get_logger(), "InvalidParameterTypeException(" << param_name << "): " << e.what());
RCLCPP_ERROR_STREAM(this->get_logger(), "Error getting parameter \'" << param_name << "\', check parameter type in YAML file");
throw e;
}
// Funktion, die kleine Bewegungen in der Twist-Nachricht filtert, um Rauschen zu reduzieren
void JoyToServoPub::_filter_twist_msg(std::unique_ptr& twist, double val)
{
// Filtere lineare und angulare Bewegungen basierend auf einem Schwellwert "val"
if (abs(twist->twist.linear.x) < val) {
twist->twist.linear.x = 0;
}
if (abs(twist->twist.linear.y) < val) {
twist->twist.linear.y = 0;
}
if (abs(twist->twist.linear.z) < val) {
twist->twist.linear.z = 0;
}
if (abs(twist->twist.angular.x) < val) {
twist->twist.angular.x = 0;
}
if (abs(twist->twist.angular.y) < val) {
twist->twist.angular.y = 0;
}
if (abs(twist->twist.angular.z) < val) {
twist->twist.angular.z = 0;
}
}
// Konvertiert die Joystick-Eingaben des Xbox 360 Controllers in Roboterkommandos
bool JoyToServoPub::_convert_xbox360_joy_to_cmd(
const std::vector& axes, const std::vector& buttons,
std::unique_ptr& twist,
std::unique_ptr& joint)
{
// Zuordnung der Achsen und Trigger-Buttons für den Xbox 360 Controller USB
int left_stick_lr = XBOX360_WIRED_LEFT_STICK_LR;
int left_stick_fb = XBOX360_WIRED_LEFT_STICK_FB;
int right_stick_lr = XBOX360_WIRED_RIGHT_STICK_LR;
int right_stick_fb = XBOX360_WIRED_RIGHT_STICK_FB;
int left_trigger = XBOX360_WIRED_LEFT_TRIGGER;
int right_trigger = XBOX360_WIRED_RIGHT_TRIGGER;
int cross_key_lr = XBOX360_WIRED_CROSS_KEY_LR;
int cross_key_fb = XBOX360_WIRED_CROSS_KEY_FB;
// Zuordnung der Buttons
int btn_a = XBOX360_BTN_A;
int btn_b = XBOX360_BTN_B;
int btn_x = XBOX360_BTN_X;
int btn_y = XBOX360_BTN_Y;
int btn_lb = XBOX360_BTN_LB;
int btn_rb = XBOX360_BTN_RB;
int btn_back = XBOX360_BTN_BACK;
int btn_start = XBOX360_BTN_START;
// Anpassung der Steuerungen basierend auf dem Controller-Typ (Wireless oder Elite 2)
if (joystick_type_ == JOYSTICK_XBOX360_WIRELESS)
{
left_stick_lr = XBOX360_WIRELESS_LEFT_STICK_LR;
left_stick_fb = XBOX360_WIRELESS_LEFT_STICK_FB;
right_stick_lr = XBOX360_WIRELESS_RIGHT_STICK_LR;
right_stick_fb = XBOX360_WIRELESS_RIGHT_STICK_FB;
left_trigger = XBOX360_WIRELESS_LEFT_TRIGGER;
right_trigger = XBOX360_WIRELESS_RIGHT_TRIGGER;
cross_key_lr = XBOX360_WIRELESS_CROSS_KEY_LR;
cross_key_fb = XBOX360_WIRELESS_CROSS_KEY_FB;
}
else if (joystick_type_ == JOYSTICK_XBOX_ONE_ELITE2)
{
left_stick_lr = XBOX_ONE_ELITE2_LEFT_STICK_LR;
left_stick_fb = XBOX_ONE_ELITE2_LEFT_STICK_FB;
right_stick_lr = XBOX_ONE_ELITE2_RIGHT_STICK_LR;
right_stick_fb = XBOX_ONE_ELITE2_RIGHT_STICK_FB;
left_trigger = XBOX_ONE_ELITE2_LEFT_TRIGGER;
right_trigger = XBOX_ONE_ELITE2_RIGHT_TRIGGER;
cross_key_lr = XBOX_ONE_ELITE2_CROSS_KEY_LR;
cross_key_fb = XBOX_ONE_ELITE2_CROSS_KEY_FB;
btn_a = XBOX_ONE_ELITE2_BTN_A;
btn_b = XBOX_ONE_ELITE2_BTN_B;
btn_x = XBOX_ONE_ELITE2_BTN_X;
btn_y = XBOX_ONE_ELITE2_BTN_Y;
btn_lb = XBOX_ONE_ELITE2_BTN_LB;
btn_rb = XBOX_ONE_ELITE2_BTN_RB;
btn_back = XBOX_ONE_ELITE2_BTN_BACK;
btn_start = XBOX_ONE_ELITE2_BTN_START;
}
// Wechsle den Planungsrahmen, wenn Back oder Start gedrückt wird
if (buttons[btn_back] && planning_frame_ == ee_frame_name_) {
planning_frame_ = robot_link_command_frame_;
}
else if (buttons[btn_start] && planning_frame_ == robot_link_command_frame_) {
planning_frame_ = ee_frame_name_;
}
// Falls eine der genannten Tasten gedrückt ist, generiere ein Gelenkbefehl
if (axes[cross_key_lr] || axes[cross_key_fb])
{
joint->joint_names.push_back("joint1");
joint->velocities.push_back(axes[cross_key_lr] * 1);
joint->joint_names.push_back("joint2");
joint->velocities.push_back(axes[cross_key_fb] * 1);
joint->joint_names.push_back("joint" + std::to_string(dof_));
joint->velocities.push_back((buttons[btn_b] - buttons[btn_x]) * 1);
// joint->joint_names.push_back("joint" + std::to_string(dof_ - 1));
// joint->velocities.push_back((buttons[btn_y] - buttons[btn_a]) * 1);
return false;
}
// Falls keine speziellen Buttons gedrückt wurden, generiere Twist-Kommandos
twist->twist.linear.x = axes[left_stick_fb];
twist->twist.linear.y = axes[left_stick_lr];
float zAchse= -.4 * (axes[left_trigger] - axes[right_trigger]);
if (zAchse >= 1) {
zAchse = 1;
} else if (zAchse <= -1) {
zAchse = -1;
}
twist->twist.linear.z = zAchse;
twist->twist.angular.y = axes[right_stick_fb];
twist->twist.angular.x = axes[right_stick_lr];
twist->twist.angular.z = buttons[btn_lb] - buttons[btn_rb];
return true;
}
// Callback-Funktion, die aufgerufen wird, wenn neue Joystick-Nachrichten empfangen werden
void JoyToServoPub::_joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg)
{
// Erstelle die Nachrichten, die veröffentlicht werden sollen (Twist und JointJog)
auto twist_msg = std::make_unique();
auto joint_msg = std::make_unique();
// Initialisierungssequenz (falls erforderlich)
if (dof_ == 7 && initialized_status_) {
initialized_status_ -= 1;
joint_msg->joint_names.push_back("joint1");
joint_msg->velocities.push_back(initialized_status_ > 0 ? 0.01 : 0);
joint_msg->header.stamp = this->now();
joint_msg->header.frame_id = "joint1";
joint_pub_->publish(std::move(joint_msg));
return;
}
bool pub_twist = false;
// Bestimme, welcher Joystick-Typ verwendet wird und konvertiere die Eingaben
switch (joystick_type_) {
case JOYSTICK_XBOX360_WIRED: // Xbox 360 mit Kabel
case JOYSTICK_XBOX360_WIRELESS: // Xbox 360 kabellos
case JOYSTICK_XBOX_ONE_ELITE2: // Xbox One Elite 2 Controller
if (msg->axes.size() >= 8 && msg->buttons.size() >= 11)
{
pub_twist = _convert_xbox360_joy_to_cmd(msg->axes, msg->buttons, twist_msg, joint_msg);
}
else
{
return;
}
break;
default:
return;
}
// Veröffentliche entweder Twist oder JointJog Nachrichten
if (pub_twist) {
_filter_twist_msg(twist_msg, 0.2);
twist_msg->header.frame_id = planning_frame_;
twist_msg->header.stamp = this->now();
twist_pub_->publish(std::move(twist_msg));
}
else {
joint_msg->header.stamp = this->now();
joint_msg->header.frame_id = "joint";
joint_pub_->publish(std::move(joint_msg));
}
// NEUE FUNKTIONALITÄT FÜR VACUUM GRIPPER TOGGLE
if (msg->buttons[XBOX_ONE_ELITE2_BTN_A] == 1) { // Überprüfe ob A-Button gedrückt ist
if (!gripper_client_->wait_for_service(std::chrono::seconds(5))) {
RCLCPP_WARN(this->get_logger(), "Service für den Vakuumgreifer nicht verfügbar.");
return;
}
auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
request->data = !greifer_status_; // Umschalten des Greifer-Status
auto result = gripper_client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS) {
greifer_status_ = request->data; // Aktualisieren des Status, wenn der Service erfolgreich war
RCLCPP_INFO(this->get_logger(), "Vakuumgreifer auf %s geschaltet", greifer_status_ ? "An" : "Aus");
} else {
RCLCPP_ERROR(this->get_logger(), "Service-Aufruf für den Vakuumgreifer fehlgeschlagen");
}
}
}
}
// Registriere die Komponente für die ROS2-Node-Komponente
include <rclcpp_components/register_node_macro.hpp>
Hello maybe someone has an idea:
i use the moveit servo script to control the xarm lite 6 with the xbox controller. so far everything works.
now i would like to use the A-button of the Xbox controller to switch the vacuum gripper on/off. i have adapted the script so far, but the function does not work properly.
what do i have to do to toggle the vacuum gripper with the A-button of the xbox controller? (i use the "Xbox One Elite 2" Controller (connected via USB) )
i changed:
`/ Copyright 2021 UFACTORY Inc. All Rights Reserved.
include "xarm_moveit_servo/xarm_joystick_input.h"
include "rclcpp/rclcpp.hpp" // ->>>>>>>>>>>>>>> NEU für vaccum gripper function
include "std_srvs/srv/set_bool.hpp" // ->>>>>>>>>>>>>>> NEU für vaccum gripper function
namespace xarm_moveit_servo {
// für verschiedene Joystick-Typen enum JOYSTICK_TYPE { JOYSTICK_XBOX360_WIRED = 1, // Xbox 360 mit Kabel JOYSTICK_XBOX360_WIRELESS = 2, // Xbox 360 kabellos JOYSTICK_XBOX_ONE_ELITE2 = 3 // Xbox One Elite 2 Controller (neu hinzugefügt) };
// für die Achsen des Xbox 360 Controllers mit Kabel enum XBOX360_WIRED_CONTROLLER_AXIS { XBOX360_WIRED_LEFT_STICK_LR = 0, // Linker Stick - Links/Rechts XBOX360_WIRED_LEFT_STICK_FB = 1, // Linker Stick - Vorwärts/Rückwärts XBOX360_WIRED_LEFT_TRIGGER = 2, // Linker Trigger XBOX360_WIRED_RIGHT_STICK_LR = 3, // Rechter Stick - Links/Rechts XBOX360_WIRED_RIGHT_STICK_FB = 4, // Rechter Stick - Vorwärts/Rückwärts XBOX360_WIRED_RIGHT_TRIGGER = 5, // Rechter Trigger XBOX360_WIRED_CROSS_KEY_LR = 6, // D-Pad - Links/Rechts XBOX360_WIRED_CROSS_KEY_FB = 7 // D-Pad - Vorwärts/Rückwärts };
// für die Achsen des kabellosen Xbox 360 Controllers enum XBOX360_WIRELESS_CONTROLLER_AXIS { XBOX360_WIRELESS_LEFT_STICK_LR = 0, XBOX360_WIRELESS_LEFT_STICK_FB = 1, XBOX360_WIRELESS_RIGHT_STICK_LR = 2, XBOX360_WIRELESS_RIGHT_STICK_FB = 3, XBOX360_WIRELESS_LEFT_TRIGGER = 4, XBOX360_WIRELESS_RIGHT_TRIGGER = 5, XBOX360_WIRELESS_CROSS_KEY_LR = 6, XBOX360_WIRELESS_CROSS_KEY_FB = 7 };
// für die Achsen des Xbox One Elite 2 Controllers enum XBOX_ONE_ELITE2_CONTROLLER_AXIS { XBOX_ONE_ELITE2_LEFT_STICK_LR = 0, XBOX_ONE_ELITE2_LEFT_STICK_FB = 1, XBOX_ONE_ELITE2_LEFT_TRIGGER = 2, XBOX_ONE_ELITE2_RIGHT_STICK_LR = 3, XBOX_ONE_ELITE2_RIGHT_STICK_FB = 4, XBOX_ONE_ELITE2_RIGHT_TRIGGER = 5, XBOX_ONE_ELITE2_CROSS_KEY_LR = 6, XBOX_ONE_ELITE2_CROSS_KEY_FB = 7 };
// für die Buttons des Xbox 360 Controllers enum XBOX360_CONTROLLER_BUTTON { XBOX360_BTN_A = 0, // A Button XBOX360_BTN_B = 1, // B Button XBOX360_BTN_X = 2, // X Button XBOX360_BTN_Y = 3, // Y Button XBOX360_BTN_LB = 4, // Left Bumper XBOX360_BTN_RB = 5, // Right Bumper XBOX360_BTN_BACK = 6, // Back Button XBOX360_BTN_START = 7, // Start Button XBOX360_BTN_POWER = 8, // Power Button XBOX360_BTN_STICK_LEFT = 9, // Linker Stick gedrückt XBOX360_BTN_STICK_RIGHT = 10 // Rechter Stick gedrückt };
// für die Buttons des Xbox One Elite 2 Controllers enum XBOX_ONE_ELITE2_CONTROLLER_BUTTON { XBOX_ONE_ELITE2_BTN_A = 0, XBOX_ONE_ELITE2_BTN_B = 1, XBOX_ONE_ELITE2_BTN_X = 2, XBOX_ONE_ELITE2_BTN_Y = 3, XBOX_ONE_ELITE2_BTN_LB = 4, XBOX_ONE_ELITE2_BTN_RB = 5, XBOX_ONE_ELITE2_BTN_BACK = 6, XBOX_ONE_ELITE2_BTN_START = 7, XBOX_ONE_ELITE2_BTN_POWER = 8, XBOX_ONE_ELITE2_BTN_STICK_LEFT = 9, XBOX_ONE_ELITE2_BTN_STICK_RIGHT = 10 };
// Konstruktor für den JoyToServoPub-Node JoyToServoPub::JoyToServoPub(const rclcpp::NodeOptions& options) : Node("joy_to_twist_publisher", options), // Initialisiert den Node mit Namen "joy_to_twistpublisher" dof(7), // Grad der Freiheit des Roboters ros_queuesize(10), // ROS-Nachrichtenwarteschlange joysticktype(JOYSTICK_XBOX360_WIRED), // Standard-Joystick ist Xbox 360 USB initializedstatus(10), // Initialisierungsstatus joytopic("/joy"), // ROS-Topic für Joystick-Nachrichten cartesian_command_intopic("/servo_server/delta_twist_cmds"), // Topic für kartesische Befehle joint_command_intopic("/servo_server/delta_joint_cmds"), // Topic für Gelenk-Befehle robot_link_commandframe("link_base"), // Basislink des Roboters ee_framename("link_eef"), // Endeffektorrahmen planningframe("link_base"), // Planungsrahmen (initial auf Basislink) greiferstatus(false) // Initialisierung des Greifer-Status { // Initialisieren der Parameter (falls nicht vorhanden, werden Standardwerte gesetzt) _declare_or_getparam(dof , "dof", dof_);
_declare_or_get_param(ros_queuesize, "ros_queue_size", ros_queuesize);
_declare_or_get_param(joysticktype, "joystick_type", joysticktype);
_declare_or_get_param(joytopic, "joy_topic", joytopic);
_declare_or_get_param(cartesian_command_intopic, "moveit_servo.cartesian_command_in_topic", cartesian_command_intopic);
_declare_or_get_param(joint_command_intopic, "moveit_servo.joint_command_in_topic", joint_command_intopic);
_declare_or_get_param(robot_link_commandframe, "moveit_servo.robot_link_command_frame", robot_link_commandframe);
_declare_or_get_param(ee_framename, "moveit_servo.ee_frame_name", ee_framename);
_declare_or_get_param(planningframe, "moveit_servo.planning_frame", planningframe);
}
// Hilfsfunktion zum Abrufen von Parametern template
void JoyToServoPub::_declare_or_get_param(T& output_value, const std::string& param_name, const T default_value)
{
try
{
if (this->has_parameter(param_name))
{
// Wenn der Parameter existiert, lese ihn aus
this->get_parameter(param_name, output_value);
}
else
{
// sonst: deklariere den Parameter mit einem Standardwert
output_value = this->declare_parameter(param_name, default_value);
}
}
catch (const rclcpp::exceptions::InvalidParameterTypeException& e)
{
RCLCPP_WARN_STREAM(this->get_logger(), "InvalidParameterTypeException(" << param_name << "): " << e.what());
RCLCPP_ERROR_STREAM(this->get_logger(), "Error getting parameter \'" << param_name << "\', check parameter type in YAML file");
throw e;
}
}
// Funktion, die kleine Bewegungen in der Twist-Nachricht filtert, um Rauschen zu reduzieren void JoyToServoPub::_filter_twist_msg(std::unique_ptr& twist, double val)
{
// Filtere lineare und angulare Bewegungen basierend auf einem Schwellwert "val"
if (abs(twist->twist.linear.x) < val) {
twist->twist.linear.x = 0;
}
if (abs(twist->twist.linear.y) < val) {
twist->twist.linear.y = 0;
}
if (abs(twist->twist.linear.z) < val) {
twist->twist.linear.z = 0;
}
if (abs(twist->twist.angular.x) < val) {
twist->twist.angular.x = 0;
}
if (abs(twist->twist.angular.y) < val) {
twist->twist.angular.y = 0;
}
if (abs(twist->twist.angular.z) < val) {
twist->twist.angular.z = 0;
}
}
// Konvertiert die Joystick-Eingaben des Xbox 360 Controllers in Roboterkommandos bool JoyToServoPub::_convert_xbox360_joy_to_cmd( const std::vector& axes, const std::vector& buttons,
std::unique_ptr& twist,
std::unique_ptr& joint)
{
// Zuordnung der Achsen und Trigger-Buttons für den Xbox 360 Controller USB
int left_stick_lr = XBOX360_WIRED_LEFT_STICK_LR;
int left_stick_fb = XBOX360_WIRED_LEFT_STICK_FB;
int right_stick_lr = XBOX360_WIRED_RIGHT_STICK_LR;
int right_stick_fb = XBOX360_WIRED_RIGHT_STICK_FB;
int left_trigger = XBOX360_WIRED_LEFT_TRIGGER;
int right_trigger = XBOX360_WIRED_RIGHT_TRIGGER;
int cross_key_lr = XBOX360_WIRED_CROSS_KEY_LR;
int cross_key_fb = XBOX360_WIRED_CROSS_KEY_FB;
}
// Callback-Funktion, die aufgerufen wird, wenn neue Joystick-Nachrichten empfangen werden void JoyToServoPub::_joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) { // Erstelle die Nachrichten, die veröffentlicht werden sollen (Twist und JointJog) auto twist_msg = std::make_unique();
auto joint_msg = std::make_unique();
}
}
// Registriere die Komponente für die ROS2-Node-Komponente
include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(xarm_moveit_servo::JoyToServoPub)`