Niko/UR_Robotiq
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix last state initial state update for the gripper

Browse Source
This commit is contained in:
ligerfotis 2024-04-16 13:47:24 +02:00
parent 7a3c6fcc2e
commit 9021f754c9
2 changed files with 12 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/servo_keyboard/package.xml
View File

@ -11,18 +11,11 @@
<!-- Dependencies --> <!-- Dependencies -->
<depend>rclcpp</depend> <depend>rclcpp</depend>
<<<<<<<< HEAD:src/servo_keyboard/package.xml
<depend>control_msgs</depend> <depend>control_msgs</depend>
<depend>geometry_msgs</depend> <depend>geometry_msgs</depend>
<build_depend>controller_manager_msgs</build_depend> <build_depend>controller_manager_msgs</build_depend>
<exec_depend>controller_manager_msgs</exec_depend> <exec_depend>controller_manager_msgs</exec_depend>
========
<depend>rclcpp_lifecycle</depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
>>>>>>>> origin/main:src/robotiq_2f/robotiq_2f_interface/package.xml
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

24
src/servo_keyboard/src/servo_keyboard_input.cpp
View File

@ -125,7 +125,7 @@ private:
void handlePlusPress(); void handlePlusPress();
void handleMinusPress(); void handleMinusPress();
void jointStateCallback(sensor_msgs::msg::JointState::SharedPtr msg); // Declaration of jointStateCallback
void publishGripperCommand(double finger_joint_angle); void publishGripperCommand(double finger_joint_angle);
rclcpp::Node::SharedPtr nh_; rclcpp::Node::SharedPtr nh_;
@ -147,7 +147,7 @@ KeyboardServo::KeyboardServo()
: frame_to_publish_(BASE_FRAME_ID), : frame_to_publish_(BASE_FRAME_ID),
joint_vel_cmd_(1.0), joint_vel_cmd_(1.0),
gripper_speed_multiplier_(0.01), // Example value, adjust as needed 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_lower_limit_(0.0), // Example value, adjust as needed
gripper_upper_limit_(0.70) // Example value, adjust as needed gripper_upper_limit_(0.70) // Example value, adjust as needed
{ {
@ -163,17 +163,17 @@ KeyboardServo::KeyboardServo()
KeyboardReader input; KeyboardReader input;
// Define callback function for joint states subscriber // Implement the jointStateCallback function
void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) void KeyboardServo::jointStateCallback(sensor_msgs::msg::JointState::SharedPtr msg)
{ {
// Find index of the finger joint in the joint state message // Find the index of the finger joint in the message
auto it = std::find(msg->name.begin(), msg->name.end(), "finger_joint"); auto it = std::find(msg->name.begin(), msg->name.end(), "finger_joint");
if (it != msg->name.end()) if (it != msg->name.end())
{ {
size_t index = std::distance(msg->name.begin(), it); size_t index = std::distance(msg->name.begin(), it);
// Update last_finger_joint_angle_ with the corresponding value from the message // Set the last finger joint angle based on the received message
last_finger_joint_angle_ = msg->position[index]; last_finger_joint_angle_ = msg->position[index];
} }
} }
void KeyboardServo::publishGripperCommand(double finger_joint_angle) void KeyboardServo::publishGripperCommand(double finger_joint_angle)