From 9295f40d74c0b4e8a5883debbb882bcf905236d2 Mon Sep 17 00:00:00 2001 From: ligerfotis Date: Thu, 18 Apr 2024 19:14:53 +0200 Subject: [PATCH] servo keyboard fixed --- .idea/vcs.xml | 2 + src/servo_keyboard/CMakeLists.txt | 17 +++++++- .../config/servo_keyboard_params.yaml | 6 +++ .../launch/servo_keyboard.launch.py | 16 +++++++ .../src/servo_keyboard_input.cpp | 42 +++++++++++++------ .../urdf/ur_robotiq.urdf.xacro | 1 - 6 files changed, 69 insertions(+), 15 deletions(-) create mode 100644 src/servo_keyboard/config/servo_keyboard_params.yaml create mode 100755 src/servo_keyboard/launch/servo_keyboard.launch.py diff --git a/.idea/vcs.xml b/.idea/vcs.xml index 4dc5475..588e956 100644 --- a/.idea/vcs.xml +++ b/.idea/vcs.xml @@ -5,5 +5,7 @@ + + \ No newline at end of file diff --git a/src/servo_keyboard/CMakeLists.txt b/src/servo_keyboard/CMakeLists.txt index 5fba1ea..1e66bf9 100644 --- a/src/servo_keyboard/CMakeLists.txt +++ b/src/servo_keyboard/CMakeLists.txt @@ -11,6 +11,9 @@ find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(control_msgs REQUIRED) find_package(controller_manager_msgs REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(ament_index_cpp REQUIRED) + add_executable(servo_keyboard_input src/servo_keyboard_input.cpp) target_include_directories(servo_keyboard_input PUBLIC @@ -23,7 +26,8 @@ target_include_directories(servo_keyboard_input PRIVATE ${rclcpp_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} ${control_msgs_INCLUDE_DIRS} - ${controller_manager_msgs_INCLUDE_DIRS} + ${controller_manager_msgs_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} # Add this line to include YAML-cpp headers ) # Link the executable with required libraries @@ -32,11 +36,22 @@ ament_target_dependencies(servo_keyboard_input geometry_msgs control_msgs controller_manager_msgs + yaml-cpp # Add this line to link against YAML-cpp + ament_index_cpp ) +target_link_libraries(servo_keyboard_input ${YAML_CPP_LIBRARIES}) # Add this line to link against YAML-cpp +# Existing install directive for the executable install(TARGETS servo_keyboard_input DESTINATION lib/${PROJECT_NAME}) +# Added install directive for the launch files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/src/servo_keyboard/config/servo_keyboard_params.yaml b/src/servo_keyboard/config/servo_keyboard_params.yaml new file mode 100644 index 0000000..a2212d2 --- /dev/null +++ b/src/servo_keyboard/config/servo_keyboard_params.yaml @@ -0,0 +1,6 @@ +/keyboard_servo: + ros__parameters: + joint_vel_cmd: 1.0 + gripper_speed_multiplier: 0.02 + gripper_lower_limit: 0.0 + gripper_upper_limit: 0.7 diff --git a/src/servo_keyboard/launch/servo_keyboard.launch.py b/src/servo_keyboard/launch/servo_keyboard.launch.py new file mode 100755 index 0000000..f08b0d0 --- /dev/null +++ b/src/servo_keyboard/launch/servo_keyboard.launch.py @@ -0,0 +1,16 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +def generate_launch_description(): + config_path = os.path.join(get_package_share_directory('servo_keyboard'), 'config', 'servo_keyboard_params.yaml') + + return LaunchDescription([ + Node( + package='servo_keyboard', + executable='servo_keyboard_input', + name='servo_keyboard_input', + output='screen', + parameters=[config_path], + ) + ]) \ No newline at end of file diff --git a/src/servo_keyboard/src/servo_keyboard_input.cpp b/src/servo_keyboard/src/servo_keyboard_input.cpp index 45f2fd1..d1b5035 100644 --- a/src/servo_keyboard/src/servo_keyboard_input.cpp +++ b/src/servo_keyboard/src/servo_keyboard_input.cpp @@ -44,6 +44,7 @@ #include // Add necessary includes #include // Add necessary includes #include // Add necessary include for JointState message +#include #include #include @@ -143,24 +144,39 @@ private: double gripper_upper_limit_; }; -KeyboardServo::KeyboardServo() - : frame_to_publish_(BASE_FRAME_ID), - joint_vel_cmd_(1.0), - gripper_speed_multiplier_(0.01), // Example value, adjust as needed -// last_finger_joint_angle_(0.0), // Example value, adjust as needed - gripper_lower_limit_(0.0), // Example value, adjust as needed - gripper_upper_limit_(0.70) // Example value, adjust as needed -{ - nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); +KeyboardServo::KeyboardServo() { - twist_pub_ = nh_->create_publisher(TWIST_TOPIC, ROS_QUEUE_SIZE); - joint_pub_ = nh_->create_publisher(JOINT_TOPIC, ROS_QUEUE_SIZE); - gripper_cmd_pub_ = nh_->create_publisher(GRIPPER_TOPIC, ROS_QUEUE_SIZE); - joint_state_sub_ = nh_->create_subscription( + // Create the node + nh_ = rclcpp::Node::make_shared("keyboard_servo"); + + // Declare the parameters + nh_->declare_parameter("joint_vel_cmd", 1.0); + nh_->declare_parameter("gripper_speed_multiplier", 0.1); + nh_->declare_parameter("gripper_lower_limit", 0.1); + nh_->declare_parameter("gripper_upper_limit", 0.4); + + // Get the parameters from the node + nh_->get_parameter("joint_vel_cmd", joint_vel_cmd_); + nh_->get_parameter("gripper_speed_multiplier", gripper_speed_multiplier_); + nh_->get_parameter("gripper_lower_limit", gripper_lower_limit_); + nh_->get_parameter("gripper_upper_limit", gripper_upper_limit_); + + // Print the parameters + printf("Joint velocity command: %f\n", joint_vel_cmd_); + printf("Gripper speed multiplier: %f\n", gripper_speed_multiplier_); + printf("Gripper lower limit: %f\n", gripper_lower_limit_); + printf("Gripper upper limit: %f\n", gripper_upper_limit_); + + twist_pub_ = nh_->create_publisher(TWIST_TOPIC, ROS_QUEUE_SIZE); + joint_pub_ = nh_->create_publisher(JOINT_TOPIC, ROS_QUEUE_SIZE); + gripper_cmd_pub_ = nh_->create_publisher(GRIPPER_TOPIC, ROS_QUEUE_SIZE); + joint_state_sub_ = nh_->create_subscription( "/joint_states", ROS_QUEUE_SIZE, std::bind(&KeyboardServo::jointStateCallback, this, std::placeholders::_1)); + } + KeyboardReader input; // Implement the jointStateCallback function diff --git a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro index f4dc290..e448c7d 100644 --- a/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro +++ b/src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro @@ -5,7 +5,6 @@ -