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 -->
<depend>rclcpp</depend>
<<<<<<<< HEAD:src/servo_keyboard/package.xml
<depend>control_msgs</depend>
<depend>geometry_msgs</depend>
<build_depend>controller_manager_msgs</build_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_common</test_depend>

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

@ -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)