diff --git a/src/servo_keyboard/package.xml b/src/servo_keyboard/package.xml index 391b053..6f3de4e 100644 --- a/src/servo_keyboard/package.xml +++ b/src/servo_keyboard/package.xml @@ -11,18 +11,11 @@ rclcpp -<<<<<<<< HEAD:src/servo_keyboard/package.xml control_msgs geometry_msgs controller_manager_msgs controller_manager_msgs -======== - rclcpp_lifecycle - hardware_interface - pluginlib ->>>>>>>> origin/main:src/robotiq_2f/robotiq_2f_interface/package.xml - ament_lint_auto ament_lint_common diff --git a/src/servo_keyboard/src/servo_keyboard_input.cpp b/src/servo_keyboard/src/servo_keyboard_input.cpp index 04cee63..45f2fd1 100644 --- a/src/servo_keyboard/src/servo_keyboard_input.cpp +++ b/src/servo_keyboard/src/servo_keyboard_input.cpp @@ -125,7 +125,7 @@ private: void handlePlusPress(); void handleMinusPress(); - + void jointStateCallback(sensor_msgs::msg::JointState::SharedPtr msg); // Declaration of jointStateCallback void publishGripperCommand(double finger_joint_angle); rclcpp::Node::SharedPtr nh_; @@ -147,7 +147,7 @@ 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 +// 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 { @@ -163,17 +163,17 @@ KeyboardServo::KeyboardServo() KeyboardReader input; -// Define callback function for joint states subscriber -void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) +// Implement the jointStateCallback function +void KeyboardServo::jointStateCallback(sensor_msgs::msg::JointState::SharedPtr msg) { - // Find index of the finger joint in the joint state message - auto it = std::find(msg->name.begin(), msg->name.end(), "finger_joint"); - if (it != msg->name.end()) - { - size_t index = std::distance(msg->name.begin(), it); - // Update last_finger_joint_angle_ with the corresponding value from the message - last_finger_joint_angle_ = msg->position[index]; - } + // Find the index of the finger joint in the message + auto it = std::find(msg->name.begin(), msg->name.end(), "finger_joint"); + if (it != msg->name.end()) + { + size_t index = std::distance(msg->name.begin(), it); + // Set the last finger joint angle based on the received message + last_finger_joint_angle_ = msg->position[index]; + } } void KeyboardServo::publishGripperCommand(double finger_joint_angle)