servo keyboard fixed

.idea/vcs.xml

@@ -5,5 +5,7 @@
     <mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Description" vcs="Git" />
     <mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Driver" vcs="Git" />
     <mapping directory="$PROJECT_DIR$/src/cartesian_controllers" vcs="Git" />
+    <mapping directory="$PROJECT_DIR$/src/ros2_robotiq_gripper" vcs="Git" />
+    <mapping directory="$PROJECT_DIR$/src/serial" vcs="Git" />
   </component>
 </project>

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

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

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],
+        )
+    ])

src/servo_keyboard/src/servo_keyboard_input.cpp

@@ -44,6 +44,7 @@
 #include <controller_manager_msgs/srv/switch_controller.hpp> // Add necessary includes
 #include <std_msgs/msg/float64_multi_array.hpp> // Add necessary includes
 #include <sensor_msgs/msg/joint_state.hpp> // Add necessary include for JointState message
+#include <ament_index_cpp/get_package_share_directory.hpp>
 
 #include <signal.h>
 #include <stdio.h>
@@ -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<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, ROS_QUEUE_SIZE);
-  joint_pub_ = nh_->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE);
-  gripper_cmd_pub_ = nh_->create_publisher<std_msgs::msg::Float64MultiArray>(GRIPPER_TOPIC, ROS_QUEUE_SIZE);
-  joint_state_sub_ = nh_->create_subscription<sensor_msgs::msg::JointState>(
+    // 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<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, ROS_QUEUE_SIZE);
+    joint_pub_ = nh_->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE);
+    gripper_cmd_pub_ = nh_->create_publisher<std_msgs::msg::Float64MultiArray>(GRIPPER_TOPIC, ROS_QUEUE_SIZE);
+    joint_state_sub_ = nh_->create_subscription<sensor_msgs::msg::JointState>(
     "/joint_states", ROS_QUEUE_SIZE,
     std::bind(&KeyboardServo::jointStateCallback, this, std::placeholders::_1));
+
 }
 
+
 KeyboardReader input;
 
 // Implement the jointStateCallback function

src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro

@@ -5,7 +5,6 @@
 	<xacro:arg name="name" default="ur_manipulator"/>
 	<!-- import main macro -->
 	<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
-	<xacro:include filename="$(find ur_robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro" />
 	<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
 
 	<!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20 -->