working keyboard example
This commit is contained in:
ligerfotis
parent
625c5082b0
commit
ad299fe757
2
.idea/.gitignore
vendored
2
.idea/.gitignore
vendored
@ -6,3 +6,5 @@
|
||||
# Datasource local storage ignored files
|
||||
/dataSources/
|
||||
/dataSources.local.xml
|
||||
# GitHub Copilot persisted chat sessions
|
||||
/copilot/chatSessions
|
||||
|
||||
25
README.md
25
README.md
@ -78,11 +78,17 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa
|
||||
```
|
||||
|
||||
### servoing
|
||||
#### PS5 controller
|
||||
Terminal 3:\
|
||||
**At the moment the servoing doesnt work with the collision checker (self collision still works).**
|
||||
```bash
|
||||
ros2 launch ur_robotiq_servo ps5_servo.launch.py
|
||||
```
|
||||
#### Keyboard
|
||||
Terminal 3:
|
||||
```bash
|
||||
ros2 run servo_keyboard servo_keyboard_input
|
||||
```
|
||||
|
||||
After launching you need to change the controller to enable the servo mode.\
|
||||
Terminal 4:
|
||||
@ -91,4 +97,23 @@ ros2 control switch_controllers --deactivate joint_trajectory_controller
|
||||
ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
|
||||
ros2 control switch_controllers --activate forward_position_controller
|
||||
ros2 control switch_controllers --activate forward_gripper_position_controller
|
||||
```
|
||||
|
||||
For the real robot you need to change to the scaled position controller:
|
||||
```bash
|
||||
ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller
|
||||
ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
|
||||
ros2 control switch_controllers --activate scaled_forward_position_controller
|
||||
ros2 control switch_controllers --activate forward_gripper_position_controller
|
||||
```
|
||||
|
||||
Terminal 5:
|
||||
Activate the Servo Node
|
||||
```bash
|
||||
ros2 service call ros2 service call /servo_node/start_servo std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
### extract the urdf from xacro file
|
||||
```bash
|
||||
ros2 run xacro xacro src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro > src/ur_robotiq_description/urdf/ur_robotiq.urdf name:=ur3e
|
||||
```
|
||||
@ -13,9 +13,8 @@ repositories:
|
||||
version: ros2
|
||||
ros2_robotiq_gripper:
|
||||
type: git
|
||||
url: https://github.com/NikoFeith/ros2_robotiq_gripper.git
|
||||
url: https://github.com/PickNikRobotics/ros2_robotiq_gripper.git
|
||||
version: main
|
||||
cartesian_controllers:
|
||||
type: git
|
||||
url: https://github.com/NikoFeith/cartesian_controllers.git
|
||||
version: ros2
|
||||
url: https://github.com/NikoFeith/cartesian_controllers.git
|
||||
@ -1,26 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(robotiq_2f_controllers)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@ -1,18 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>robotiq_2f_controllers</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,26 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(robotiq_2f_description)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@ -1,9 +0,0 @@
|
||||
robotiq_2f_140_gripper:
|
||||
ros__parameters:
|
||||
# Physical limits
|
||||
min_position: 0.0 # Meters (fully closed)
|
||||
max_position: 0.14 # Meters (fully open)
|
||||
min_speed: 0.02 # Meters per second
|
||||
max_speed: 0.15 # Meters per second
|
||||
min_force: 20.0 # Newtons
|
||||
max_force: 235.0 # Newtons
|
||||
@ -1,11 +0,0 @@
|
||||
robotiq_2f_140_gripper:
|
||||
ros__parameters:
|
||||
robot_ip: "192.168.1.11"
|
||||
robot_port: 63352
|
||||
# Physical limits
|
||||
min_position: 0.0 # Meters (fully closed)
|
||||
max_position: 0.085 # Meters (fully open)
|
||||
min_speed: 0.02 # Meters per second
|
||||
max_speed: 0.15 # Meters per second
|
||||
min_force: 20.0 # Newtons
|
||||
max_force: 235.0 # Newtons
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@ -1,18 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>robotiq_2f_description</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,87 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="robotiq_gripper_ros2_control" params="
|
||||
name
|
||||
prefix
|
||||
use_fake_hardware:=false
|
||||
fake_sensor_commands:=false
|
||||
robot_ip:=192.168.1.1
|
||||
robot_port:=63352>
|
||||
|
||||
<ros2_control name="${name}" type="system">
|
||||
<!-- Plugins -->
|
||||
<hardware>
|
||||
<xacro:if value="${use_fake_hardware}">
|
||||
<plugin>mock_components/GenericSystem</plugin>
|
||||
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
|
||||
<param name="state_following_offset">0.0</param>
|
||||
</xacro:if>
|
||||
<xacro:unless value="${use_fake_hardware}">
|
||||
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
|
||||
<param name="gripper_closed_position">0.14</param>
|
||||
<param name="robot_ip">${robot_ip}</param>
|
||||
<param name="robot_port">${robot_port}</param>
|
||||
<param name="gripper_speed_multiplier">1.0</param>
|
||||
<param name="gripper_force_multiplier">0.5</param>
|
||||
</xacro:unless>
|
||||
</hardware>
|
||||
|
||||
<!-- Joint interfaces -->
|
||||
<!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
|
||||
<joint name="${prefix}finger_joint">
|
||||
<command_interface name="position" />
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">0.14</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<!-- When simulating we need to include the rest of the gripper joints -->
|
||||
<xacro:if value="${use_fake_hardware}">
|
||||
<joint name="${prefix}left_inner_knuckle_joint">
|
||||
<param name="mimic">${prefix}finger_joint</param>
|
||||
<param name="multiplier">-1</param>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${prefix}left_inner_finger_joint">
|
||||
<param name="mimic">${prefix}finger_joint</param>
|
||||
<param name="multiplier">1</param>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${prefix}right_outer_knuckle_joint">
|
||||
<param name="mimic">${prefix}finger_joint</param>
|
||||
<param name="multiplier">-1</param>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${prefix}robotiq_140_left_finger_tip_joint">
|
||||
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
|
||||
<param name="multiplier">-1</param>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="${prefix}robotiq_140_right_finger_tip_joint">
|
||||
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
|
||||
<param name="multiplier">1</param>
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
</xacro:if>
|
||||
|
||||
<!-- Only add this with fake hardware mode -->
|
||||
<gpio name="reactivate_gripper">
|
||||
<command_interface name="reactivate_gripper_cmd" />
|
||||
<command_interface name="reactivate_gripper_response" />
|
||||
</gpio>
|
||||
|
||||
</ros2_control>
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
@ -1,28 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(robotiq_2f_interface)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(hardware_interface REQUIRED)
|
||||
find_package(lifecycle_msgs REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@ -1,356 +0,0 @@
|
||||
// Robotiq2fSocketAdapter.hpp
|
||||
|
||||
#ifndef ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||
#define ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <sys/time.h>
|
||||
#include <sstream>
|
||||
#include <tuple>
|
||||
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
#include <unistd.h> // For close()
|
||||
#include <cstring> // For memset()
|
||||
#include <sys/select.h> // For select() system call
|
||||
|
||||
#include <stdexcept> // For std::runtime_error
|
||||
#include <cerrno> // For errno
|
||||
#include <iostream>
|
||||
|
||||
// Enum for Gripper Status
|
||||
enum class GripperStatus {
|
||||
RESET = 0,
|
||||
ACTIVATING = 1,
|
||||
// UNUSED = 2, // This value is currently not used by the gripper firmware
|
||||
ACTIVE = 3
|
||||
};
|
||||
|
||||
// Enum for Object Status
|
||||
enum class ObjectStatus {
|
||||
MOVING = 0,
|
||||
STOPPED_OUTER_OBJECT = 1,
|
||||
STOPPED_INNER_OBJECT = 2,
|
||||
AT_DEST = 3
|
||||
};
|
||||
|
||||
class Robotiq2fSocketAdapter {
|
||||
public:
|
||||
|
||||
Robotiq2fSocketAdapter() ;
|
||||
~Robotiq2fSocketAdapter();
|
||||
|
||||
// Connection management
|
||||
/**
|
||||
* Establishes a TCP connection to the Robotiq gripper.
|
||||
*
|
||||
* Attempts to open a socket and connect to the specified hostname and port. If the adapter
|
||||
* is already connected, it will disconnect and attempt to reconnect.
|
||||
*
|
||||
* @param hostname The IP address or hostname of the Robotiq gripper.
|
||||
* @param port The port number on which the gripper is listening for connections.
|
||||
* @return True if the connection was successfully established, False otherwise.
|
||||
*
|
||||
* @throws std::runtime_error if socket creation fails.
|
||||
*/
|
||||
bool connect(const std::string& hostname, int port);
|
||||
/**
|
||||
* Closes the connection to the Robotiq gripper.
|
||||
*
|
||||
* Safely shuts down and closes the current socket connection to the gripper. If the socket
|
||||
* is already closed or was never connected, this method has no effect. This method is
|
||||
* thread-safe and can be called concurrently with other operations on the adapter.
|
||||
*/
|
||||
void disconnect();
|
||||
|
||||
|
||||
/**
|
||||
* Sets multiple gripper variables using a map of variable names to integer/double values.
|
||||
*
|
||||
* This method allows setting multiple configuration parameters of the gripper simultaneously.
|
||||
* The variable names must match the expected variable identifiers used by the gripper.
|
||||
* Integer/double values are used directly in the command sent to the gripper.
|
||||
*
|
||||
* @param variableMap A map where each key is a string representing the variable name and
|
||||
* each value is an integer representing the variable's desired value.
|
||||
* @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
|
||||
*/
|
||||
bool setGripperVariables(const std::map<std::string, int>& variableMap);
|
||||
bool setGripperVariables(const std::map<std::string, double>& variableMap);
|
||||
|
||||
/**
|
||||
* Sets a single gripper variable.
|
||||
*
|
||||
* Allows setting a single configuration parameter of the gripper. The method overloads allow
|
||||
* specifying the variable value as either an integer or a double, depending on the expected
|
||||
* variable type.
|
||||
*
|
||||
* @param variable The variable name, matching the gripper's expected variable identifiers.
|
||||
* @param value The desired value for the variable, either as an int or a double.
|
||||
* @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
|
||||
*/
|
||||
bool setGripperVariable(const std::string& variable, int value);
|
||||
bool setGripperVariable(const std::string& variable, double value);
|
||||
|
||||
/**
|
||||
* Retrieves the current value of a specified gripper variable.
|
||||
*
|
||||
* Sends a command to the gripper to report the current value of a specific configuration parameter.
|
||||
* The method parses the gripper's response to extract the variable value.
|
||||
*
|
||||
* @param variable The variable name, matching the gripper's expected variable identifiers.
|
||||
* @return The current value of the variable as an integer. Returns -1 if the command fails or
|
||||
* if the variable is not recognized by the gripper.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper, if sending the command fails,
|
||||
* or if the response from the gripper is malformed.
|
||||
*/
|
||||
int getGripperVariable(const std::string& variable);
|
||||
|
||||
// Gripper status
|
||||
bool isGripperActive();
|
||||
int getGripperPosition();
|
||||
int getGripperForce();
|
||||
int force();
|
||||
int speed();
|
||||
int position();
|
||||
|
||||
/**
|
||||
* Checks if the gripper is currently active.
|
||||
*
|
||||
* Queries the current state of the gripper to determine if it is active. This method interprets
|
||||
* the gripper's status based on the last known communication and does not initiate a new request
|
||||
* to the gripper.
|
||||
*
|
||||
* @return True if the gripper is in the ACTIVE state, False otherwise.
|
||||
*
|
||||
* @throws std::runtime_error if unable to retrieve the gripper's status, typically due to communication issues.
|
||||
*/
|
||||
bool is_active();
|
||||
|
||||
|
||||
// Activates the gripper and optionally performs auto-calibration
|
||||
/**
|
||||
* Activates the Robotiq gripper and optionally performs auto-calibration.
|
||||
*
|
||||
* This method sends an activation command to the gripper, bringing it from a RESET or INACTIVE
|
||||
* state to an ACTIVE state. If autoCalibrate is set to true, it also initiates an auto-calibration
|
||||
* procedure to determine the maximum and minimum positions of the gripper's motion range.
|
||||
*
|
||||
* @param autoCalibrate A boolean flag indicating whether to perform auto-calibration after
|
||||
* activation. Default is true.
|
||||
*
|
||||
* @note Activation and auto-calibration may take several seconds to complete. The method
|
||||
* waits for the gripper to acknowledge activation before returning.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper, if sending the
|
||||
* activation command fails, or if the gripper does not reach the expected state within
|
||||
* a reasonable timeframe.
|
||||
*/
|
||||
void activate(bool autoCalibrate = true);
|
||||
/**
|
||||
* Performs auto-calibration of the gripper.
|
||||
*
|
||||
* This method initiates an auto-calibration sequence where the gripper fully opens and closes to determine
|
||||
* its operational range. This is useful for ensuring accurate position control, especially if the mechanical
|
||||
* setup of the gripper has been altered or if it is being used for the first time.
|
||||
*
|
||||
* @param log A boolean flag indicating whether to log the calibration results. Defaults to true. When set
|
||||
* to true, the final calibration values will be logged to the standard output.
|
||||
*
|
||||
* @note This method assumes the gripper is already activated. If the gripper is not in an active state,
|
||||
* the calibration sequence may fail. Ensure the gripper is activated before calling this method.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper, if the gripper is not in an
|
||||
* active state, or if the calibration sequence fails.
|
||||
*/
|
||||
void autoCalibrate(bool log=true);
|
||||
|
||||
/**
|
||||
* Deactivates the gripper.
|
||||
*
|
||||
* Sends a command to transition the gripper from an active state back to a reset state. This method
|
||||
* is useful for safely shutting down the gripper or preparing it for a state change.
|
||||
*
|
||||
* @note This method should be called before disconnecting from the gripper or when the gripper is not
|
||||
* needed for an extended period to ensure it is left in a safe state.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper or if the deactivation command fails.
|
||||
*/
|
||||
void deactivate();
|
||||
|
||||
// Move Commands
|
||||
/**
|
||||
* Commands the gripper to move to a specified position.
|
||||
*
|
||||
* Instructs the gripper to move to a given position with specified speed and force. The position,
|
||||
* speed, and force parameters are automatically clipped to the valid range supported by the gripper.
|
||||
*
|
||||
* @param position The target position for the gripper, typically in the range [0, 255].
|
||||
* @param speed The speed at which the gripper should move, typically in the range [0, 255].
|
||||
* @param force The force to be applied by the gripper, typically in the range [0, 255].
|
||||
* @return A tuple containing a boolean indicating success of the command, and an integer representing the clipped target position.
|
||||
*
|
||||
* @throws std::runtime_error if there's an error in sending the move command or if the adapter is not connected to a gripper.
|
||||
*/
|
||||
std::tuple<bool, int> move(int position, int speed, int force);
|
||||
/**
|
||||
* Commands the gripper to move to a specified position and waits for the move to complete.
|
||||
*
|
||||
* This method instructs the gripper to move to the given position with specified speed and force,
|
||||
* then blocks until the gripper acknowledges reaching the target position or stops moving due to
|
||||
* detecting an object.
|
||||
*
|
||||
* @param position The target position for the gripper, typically in the range [0, 255].
|
||||
* @param speed The speed at which the gripper should move, typically in the range [0, 255].
|
||||
* @param force The force to be applied by the gripper, typically in the range [0, 255].
|
||||
* @return A tuple containing the final position of the gripper as an integer and the object status
|
||||
* as an `ObjectStatus` enumeration value.
|
||||
*
|
||||
* @throws std::runtime_error if there's an error in sending the move command, if the adapter is not
|
||||
* connected to a gripper, or if the gripper does not reach the target position within a
|
||||
* reasonable time frame.
|
||||
*/
|
||||
std::tuple<int, ObjectStatus> move_and_wait_for_pos(int position, int speed, int force);
|
||||
|
||||
private:
|
||||
// Custom deleter for the smart pointer managing the socket descriptor
|
||||
struct SocketDeleter {
|
||||
void operator()(int* ptr) {
|
||||
if (ptr && *ptr >= 0) {
|
||||
close(*ptr);
|
||||
}
|
||||
delete ptr;
|
||||
}
|
||||
};
|
||||
|
||||
// Smart pointer to manage the socket descriptor with the custom deleter
|
||||
std::unique_ptr<int, SocketDeleter> _sockfd;
|
||||
std::mutex socketMutex; // Mutex for socket access synchronization
|
||||
|
||||
// bounds of the encoded gripper states
|
||||
int _min_position = 0;
|
||||
int _max_position = 255;
|
||||
int _min_speed = 0;
|
||||
int _max_speed = 255;
|
||||
int _min_force = 0;
|
||||
int _max_force = 255;
|
||||
|
||||
int open_position = _min_position;
|
||||
int closed_position = _max_position;
|
||||
|
||||
|
||||
/**
|
||||
* Sends a command to the connected Robotiq gripper.
|
||||
*
|
||||
* Constructs and sends a command string to the gripper over the established socket connection.
|
||||
* This method ensures thread-safe access to the socket and handles low-level socket write operations.
|
||||
*
|
||||
* @param cmd The command string to be sent to the gripper. Must conform to the gripper's protocol.
|
||||
* @return True if the command was successfully sent, False if there was an error sending the command.
|
||||
*
|
||||
* @throws std::runtime_error if attempted on a disconnected socket.
|
||||
*/
|
||||
bool sendCommand(const std::string& cmd);
|
||||
|
||||
/**
|
||||
* Receives a response from the Robotiq gripper.
|
||||
*
|
||||
* Waits for and reads a response from the gripper within a specified timeout period. This method
|
||||
* is blocking but allows specifying a timeout to prevent indefinite waiting. It ensures thread-safe
|
||||
* access to the socket and handles low-level socket read operations.
|
||||
*
|
||||
* @param timeout_ms The timeout in milliseconds to wait for a response. Defaults to 2000 milliseconds.
|
||||
* @return A string containing the response from the gripper. If the operation times out or fails, an empty string is returned.
|
||||
*
|
||||
* @throws std::runtime_error if attempted on a disconnected socket or if a select() call fails.
|
||||
*/
|
||||
std::string receiveResponse(int timeout_ms=2000);
|
||||
|
||||
|
||||
/**
|
||||
* Checks if the received data is an acknowledgement message from the gripper.
|
||||
*
|
||||
* This method parses the received data string to determine if it matches the expected acknowledgement
|
||||
* pattern or message, indicating that a previously sent command was received and executed by the gripper.
|
||||
*
|
||||
* @param data The raw string data received from the gripper.
|
||||
* @return True if the data represents an acknowledgement message, False otherwise.
|
||||
*/
|
||||
bool isAck(const std::string& data);
|
||||
|
||||
// Helper Functions
|
||||
/**
|
||||
* Clips a variable to ensure it falls within a specified range.
|
||||
*
|
||||
* This helper function ensures that a given value does not exceed the minimum and maximum bounds
|
||||
* defined for it. It is particularly useful for sanitizing command parameters like position, speed,
|
||||
* and force values before sending them to the gripper.
|
||||
*
|
||||
* @param min_value The minimum allowable value for the variable.
|
||||
* @param variable The current value of the variable to be clipped.
|
||||
* @param max_value The maximum allowable value for the variable.
|
||||
* @return The clipped value, constrained within the specified min and max bounds.
|
||||
*/
|
||||
int clip_val(int min_value, int variable, int max_value);
|
||||
/**
|
||||
* Blocks until the gripper reaches a specified status.
|
||||
*
|
||||
* This method polls the gripper's status at regular intervals until it matches the expected status
|
||||
* or until a timeout occurs. It is used internally to synchronize actions like activation, deactivation,
|
||||
* and calibration with the gripper's state transitions.
|
||||
*
|
||||
* @param expectedStatus The GripperStatus enumeration value the method waits for the gripper to achieve.
|
||||
*
|
||||
* @throws std::runtime_error if the gripper does not reach the expected status within a reasonable time frame,
|
||||
* indicating a possible communication issue or a failure in executing the command.
|
||||
*/
|
||||
void waitForGripperStatus(GripperStatus expectedStatus);
|
||||
|
||||
// Resets the gripper
|
||||
/**
|
||||
* Resets the gripper to its initial state.
|
||||
*
|
||||
* Sends a command to the gripper to transition it to the RESET state. This is often used as a first step
|
||||
* in a sequence of commands that prepare the gripper for operation, ensuring it starts from a known state.
|
||||
*
|
||||
* @note This method can be used to recover the gripper from error states or to prepare it for reactivation.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper or if the reset command fails.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
// Define the command strings and encoding used for communication
|
||||
const std::string SET_COMMAND = "SET";
|
||||
const std::string GET_COMMAND = "GET";
|
||||
const std::string ACK_MESSAGE = "ack";
|
||||
|
||||
// Commands
|
||||
const std::string CMD_ACT = "ACT"; // Activation
|
||||
const std::string CMD_GTO = "GTO"; // GoTo
|
||||
const std::string CMD_ATR = "ATR"; // Automatic Release
|
||||
const std::string CMD_ADR = "ADR"; // Advanced Control
|
||||
const std::string CMD_FOR = "FOR"; // Force
|
||||
const std::string CMD_SPE = "SPE"; // Speed
|
||||
const std::string CMD_POS = "POS"; // Position
|
||||
const std::string CMD_STA = "STA"; // Status
|
||||
const std::string CMD_PRE = "PRE"; // Position Request
|
||||
const std::string CMD_OBJ = "OBJ"; // Object Detection
|
||||
const std::string CMD_FLT = "FLT"; // Fault
|
||||
|
||||
// Constants buffer sizes
|
||||
const size_t BUFFER_SIZE = 1024; // Adjust as necessary
|
||||
};
|
||||
|
||||
#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||
@ -1,104 +0,0 @@
|
||||
// hardware_interface.hpp
|
||||
|
||||
#ifndef ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||
#define ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
|
||||
#include <Robotiq2fSocketAdapter.hpp>
|
||||
|
||||
#include <hardware_interface/actuator_interface.hpp>
|
||||
#include <hardware_interface/types/hardware_interface_type_values.hpp>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <urdf/model.h>
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
namespace robotiq_driver
|
||||
{
|
||||
class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
|
||||
{
|
||||
public:
|
||||
// Constructors
|
||||
ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface();
|
||||
ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface();
|
||||
// ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr<SocketFactory> socket_factory);
|
||||
|
||||
// Lifecycle Management
|
||||
ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
|
||||
ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
|
||||
ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
|
||||
ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
|
||||
|
||||
// State and Command Interfaces
|
||||
ROBOTIQ_DRIVER_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
ROBOTIQ_DRIVER_PUBLIC std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
|
||||
|
||||
// Data Access (Read/Write)
|
||||
ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||
ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||
|
||||
protected: // Likely changes the access to protected from private
|
||||
// Members for driver interaction
|
||||
std::unique_ptr<Robotiq2fSocketAdapter> socket_adapter_;
|
||||
// std::unique_ptr<DriverFactory> driver_factory_;
|
||||
|
||||
// Background communication thread
|
||||
std::thread communication_thread_;
|
||||
std::atomic<bool> communication_thread_is_running_;
|
||||
void background_task();
|
||||
|
||||
static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
|
||||
|
||||
// Conversion Methods
|
||||
double convertRawToPosition(int raw_position);
|
||||
int convertPositionToRaw(double position);
|
||||
int convertSpeedToRaw(double speed);
|
||||
int convertForceToRaw(double force);
|
||||
|
||||
// Gripper Commands
|
||||
std::atomic<uint8_t> write_command_;
|
||||
std::atomic<uint8_t> write_force_;
|
||||
std::atomic<uint8_t> write_speed_;
|
||||
std::atomic<uint8_t> gripper_current_state_;
|
||||
|
||||
|
||||
// Gripper State Tracking
|
||||
double gripper_position_ = 0.0;
|
||||
double gripper_velocity_ = 0.0;
|
||||
double gripper_position_command_ = 0.0;
|
||||
|
||||
double reactivate_gripper_cmd_ = 0.0;
|
||||
std::atomic<bool> reactivate_gripper_async_cmd_;
|
||||
double reactivate_gripper_response_ = 0.0;
|
||||
double gripper_force_ = 0.0;
|
||||
double gripper_speed_ = 0.0;
|
||||
std::atomic<std::optional<bool>> reactivate_gripper_async_response_;
|
||||
|
||||
// Connection Variables
|
||||
std::string robot_ip_;
|
||||
int robot_port_;
|
||||
|
||||
// Parameters for physical limits and configuration
|
||||
double min_position_;
|
||||
double max_position_;
|
||||
double min_speed_;
|
||||
double max_speed_;
|
||||
double min_force_;
|
||||
double max_force_;
|
||||
|
||||
// loop time
|
||||
constexpr auto gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
|
||||
|
||||
// Logger
|
||||
const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
|
||||
};
|
||||
}
|
||||
|
||||
#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||
@ -1,369 +0,0 @@
|
||||
#include "Robotiq2fSocketAdapter.hpp"
|
||||
|
||||
|
||||
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : _sockfd(new int(-1)) {
|
||||
}
|
||||
|
||||
|
||||
Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
|
||||
disconnect(); // Ensure the socket is closed properly.
|
||||
}
|
||||
|
||||
// Connection and disconnection
|
||||
bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
|
||||
std::lock_guard<std::mutex> lock(socketMutex);
|
||||
if (*_sockfd >= 0) {
|
||||
std::cerr << "Already connected. Disconnecting to reconnect.\n";
|
||||
disconnect();
|
||||
}
|
||||
|
||||
int sock = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sock < 0) {
|
||||
std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
|
||||
return false;
|
||||
}
|
||||
*_sockfd = sock;
|
||||
|
||||
sockaddr_in serv_addr{};
|
||||
memset(&serv_addr, 0, sizeof(serv_addr));
|
||||
serv_addr.sin_family = AF_INET;
|
||||
serv_addr.sin_port = htons(port);
|
||||
|
||||
if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) {
|
||||
std::cerr << "Invalid address: " << hostname << "\n";
|
||||
close(*_sockfd);
|
||||
*_sockfd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (::connect(_sockfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
|
||||
std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
|
||||
close(*_sockfd);
|
||||
*_sockfd = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Robotiq2fSocketAdapter::disconnect() {
|
||||
std::lock_guard<std::mutex> lock(socketMutex);
|
||||
if (*_sockfd >= 0) {
|
||||
if (close(*_sockfd) == -1) {
|
||||
std::cerr << "Error closing socket: " << strerror(errno) << "\n";
|
||||
} else {
|
||||
std::cout << "Disconnected successfully.\n";
|
||||
}
|
||||
*_sockfd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Utility methods
|
||||
bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
|
||||
std::lock_guard<std::mutex> lock(socketMutex);
|
||||
if (*_sockfd < 0) {
|
||||
std::cerr << "Attempted to send command on a disconnected socket.\n";
|
||||
return false;
|
||||
}
|
||||
|
||||
ssize_t result = send(*_sockfd, cmd.c_str(), cmd.length(), 0);
|
||||
if (result < 0) {
|
||||
std::cerr << "Failed to send command: " << strerror(errno) << "\n";
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) {
|
||||
std::lock_guard<std::mutex> lock(socketMutex);
|
||||
if (*_sockfd < 0) {
|
||||
std::cerr << "Attempted to receive response on a disconnected socket.\n";
|
||||
return "";
|
||||
}
|
||||
|
||||
std::string result;
|
||||
char buffer[BUFFER_SIZE];
|
||||
bool complete_response = false;
|
||||
|
||||
timeval tv;
|
||||
tv.tv_sec = timeout_ms / 1000;
|
||||
tv.tv_usec = (timeout_ms % 1000) * 1000;
|
||||
|
||||
// Loop to receive data until a complete response is obtained or a timeout occurs
|
||||
while (!complete_response) {
|
||||
fd_set readfds;
|
||||
FD_ZERO(&readfds);
|
||||
FD_SET(*_sockfd, &readfds);
|
||||
|
||||
int activity = select(*_sockfd + 1, &readfds, nullptr, nullptr, &tv);
|
||||
if (activity == -1) {
|
||||
std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
|
||||
return "";
|
||||
} else if (activity == 0) {
|
||||
std::cerr << "Timeout occurred while waiting for response.\n";
|
||||
return "";
|
||||
}
|
||||
|
||||
ssize_t bytes_read = recv(*_sockfd, buffer, BUFFER_SIZE - 1, 0);
|
||||
if (bytes_read < 0) {
|
||||
if (errno != EAGAIN && errno != EWOULDBLOCK) {
|
||||
std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
|
||||
}
|
||||
return "";
|
||||
} else if (bytes_read == 0) {
|
||||
std::cerr << "Connection closed by peer.\n";
|
||||
return "";
|
||||
} else {
|
||||
buffer[bytes_read] = '\0';
|
||||
result += buffer;
|
||||
|
||||
// Check if we have a complete response:
|
||||
if (result.find("\n") != std::string::npos) {
|
||||
complete_response = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
bool Robotiq2fSocketAdapter::isAck(const std::string& data) {
|
||||
return data == ACK_MESSAGE;
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
|
||||
return std::max(min_value, std::min(value, max_value));
|
||||
}
|
||||
|
||||
void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) {
|
||||
int retries = 0;
|
||||
int status = getGripperVariable(CMD_STA);
|
||||
while (static_cast<GripperStatus>(status) != expectedStatus && retries < MAX_RETRIES) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(RETRY_DELAY_MS));
|
||||
status = getGripperVariable(CMD_STA);
|
||||
retries++;
|
||||
}
|
||||
if (retries >= MAX_RETRIES) {
|
||||
throw std::runtime_error("Gripper failed to reach expected status.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Gripper variable Getter and Setter
|
||||
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, int>& variableMap) {
|
||||
return setGripperVariables(reinterpret_cast<const std::map<std::string, double>&>(variableMap));
|
||||
}
|
||||
|
||||
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, double>& variableMap) {
|
||||
// Build the command string
|
||||
std::string cmd = SET_COMMAND;
|
||||
for (const auto& [variable, value] : variableMap) {
|
||||
std::stringstream ss;
|
||||
ss << value;
|
||||
cmd += " " + commandVariables.at(variable) + " " + ss.str();
|
||||
}
|
||||
cmd += "\n";
|
||||
|
||||
// Send the command and receive the response
|
||||
std::lock_guard<std::mutex> lock(socketMutex);
|
||||
if (*_sockfd < 0) {
|
||||
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!sendCommand(cmd)) {
|
||||
throw std::runtime_error("[setGripperVariables] Sending command failed.");
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string response = receiveResponse();
|
||||
return isAck(response);
|
||||
}
|
||||
|
||||
bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, int value) {
|
||||
return setGripperVariable(variable, static_cast<double>(value)); // Convert int to double
|
||||
}
|
||||
|
||||
bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) {
|
||||
std::stringstream ss;
|
||||
ss << value; // Convert the value to a string
|
||||
|
||||
std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n";
|
||||
|
||||
std::lock_guard<std::mutex> lock(socketMutex);
|
||||
if (*_sockfd < 0) {
|
||||
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!sendCommand(cmd)) {
|
||||
throw std::runtime_error("[setGripperVariable] Sending command failed.");
|
||||
return false; // Sending command failed
|
||||
}
|
||||
|
||||
std::string response = receiveResponse();
|
||||
return isAck(response);
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
|
||||
std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n";
|
||||
|
||||
std::lock_guard<std::mutex> lock(socketMutex);
|
||||
if (*_sockfd < 0) {
|
||||
throw std::runtime_error("Cannot get variables on a disconnected socket.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!sendCommand(cmd)) {
|
||||
throw std::runtime_error("[getGripperVariable] Sending command failed.");
|
||||
return -1; // Error sending command
|
||||
}
|
||||
|
||||
std::string response = receiveResponse();
|
||||
|
||||
// Parse the response (assuming format: "variable_name value")
|
||||
std::istringstream iss(response);
|
||||
std::string var_name, value_str;
|
||||
|
||||
if (!(iss >> var_name >> value_str)) {
|
||||
throw std::runtime_error("Invalid gripper response format.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (var_name != variable) {
|
||||
throw std::runtime_error("Unexpected response - variable name mismatch.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
try {
|
||||
return std::stoi(value_str);
|
||||
} catch (const std::exception& ex) {
|
||||
std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Activation, deactivation and reset of the Gripper
|
||||
void Robotiq2fSocketAdapter::activate(bool autoCalibrate) {
|
||||
reset(); // Always start with a reset to ensure a known state
|
||||
setGripperVariable(CMD_ACT, 1); // Activate the gripper
|
||||
waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active
|
||||
|
||||
if (autoCalibrate) {
|
||||
auto_calibrate();
|
||||
}
|
||||
}
|
||||
|
||||
void Robotiq2fSocketAdapter::reset() {
|
||||
setGripperVariable(CMD_ACT, 0);
|
||||
setGripperVariable(CMD_ATR, 0);
|
||||
waitForGripperStatus(GripperStatus::RESET);
|
||||
}
|
||||
|
||||
|
||||
void Robotiq2fSocketAdapter::deactivate() {
|
||||
if (is_active()) {
|
||||
reset();
|
||||
}
|
||||
}
|
||||
|
||||
// State subsscriber
|
||||
bool Robotiq2fSocketAdapter::is_active(){
|
||||
GripperStatus status = static_cast<GripperStatus>(getGripperVariable(CMD_STA));
|
||||
return GripperStatus::ACTIVE == status;
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::force(){
|
||||
return getGripperVariable(CMD_FOR);
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::speed(){
|
||||
return getGripperVariable(CMD_SPE);
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::position(){
|
||||
return getGripperVariable(CMD_POS);
|
||||
}
|
||||
|
||||
// Movement Methods
|
||||
std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
|
||||
int clippedPosition = clip_val(_min_position, position, _max_position);
|
||||
int clippedSpeed = clip_val(_min_speed, speed, _max_speed);
|
||||
int clippedForce = clip_val(_min_force, force, _max_force);
|
||||
|
||||
// Create a map for gripper variables (similar to Python's OrderedDict)
|
||||
std::map<std::string, int> variableMap = {
|
||||
{ CMD_POS, clippedPosition },
|
||||
{ CMD_SPE, clippedSpeed },
|
||||
{ CMD_FOR, clippedForce },
|
||||
{ CMD_GTO, 1 }
|
||||
};
|
||||
|
||||
bool setResult = setGripperVariables(variableMap);
|
||||
return std::make_tuple(setResult, clippedPosition);
|
||||
}
|
||||
|
||||
std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
|
||||
int clippedPosition = clip_val(min_position, position, max_position);
|
||||
int clippedSpeed = clip_val(min_speed, speed, max_speed);
|
||||
int clippedForce = clip_val(min_force, force, max_force);
|
||||
|
||||
// Set gripper variables to initiate the move
|
||||
std::map<std::string, int> variableMap = {
|
||||
{ CMD_POS, clippedPosition },
|
||||
{ CMD_SPE, clippedSpeed },
|
||||
{ CMD_FOR, clippedForce },
|
||||
{ CMD_GTO, 1 }
|
||||
};
|
||||
bool setResult = setGripperVariables(variableMap);
|
||||
if (!setResult) {
|
||||
// Handle the error case, e.g., throw an exception
|
||||
throw std::runtime_error("Failed to set variables for move.");
|
||||
}
|
||||
|
||||
// Wait until position is acknowledged:
|
||||
while(getGripperVariable(CMD_PRE) != clippedPosition) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
|
||||
}
|
||||
|
||||
// Wait until not moving
|
||||
int currentObjectStatus = getGripperVariable(CMD_OBJ);
|
||||
while (ObjectStatus(currentObjectStatus) == ObjectStatus::MOVING) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
|
||||
currentObjectStatus = getGripperVariable(CMD_OBJ);
|
||||
}
|
||||
|
||||
// Report final position and object status
|
||||
int finalPosition = getGripperVariable(CMD_POS);
|
||||
ObjectStatus finalStatus = static_cast<ObjectStatus>(currentObjectStatus);
|
||||
return std::make_tuple(finalPosition, finalStatus);
|
||||
}
|
||||
|
||||
|
||||
void Robotiq2fSocketAdapter::auto_calibrate(bool log=true) {
|
||||
// Open in case we are holding an object
|
||||
std::tuple<int, ObjectStatus> result = move_and_wait_for_pos(open_position, 64, 1);
|
||||
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||
throw std::runtime_error("Calibration failed opening to start.");
|
||||
}
|
||||
|
||||
// Close as far as possible
|
||||
result = move_and_wait_for_pos(closed_position, 64, 1);
|
||||
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||
throw std::runtime_error("Calibration failed because of an object.");
|
||||
}
|
||||
_max_position = std::get<0>(result); // Update max position
|
||||
|
||||
// Open as far as possible
|
||||
result = move_and_wait_for_pos(open_position, 64, 1);
|
||||
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||
throw std::runtime_error("Calibration failed because of an object.");
|
||||
}
|
||||
_min_position = std::get<0>(result); // Update min position
|
||||
|
||||
if (log) {
|
||||
std::cout << "Gripper auto-calibrated to ["
|
||||
<< _min_position << ", " << _max_position << "]\n";
|
||||
}
|
||||
}
|
||||
@ -1,385 +0,0 @@
|
||||
#include "RobotiqGripperHardwareInterface.hpp"
|
||||
|
||||
namespace robotiq_driver
|
||||
{
|
||||
RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
|
||||
: communication_thread_is_running_(false)
|
||||
{
|
||||
// socket_factory_ = std::make_unique<DefaultSocketFactory>();
|
||||
}
|
||||
|
||||
RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
|
||||
{
|
||||
if (communication_thread_is_running_.load())
|
||||
{
|
||||
communication_thread_is_running_.store(false);
|
||||
if (communication_thread_.joinable())
|
||||
{
|
||||
communication_thread_.join();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
|
||||
{
|
||||
RCLCPP_DEBUG(rclcpp::get_logger("RobotiqGripperHardwareInterface"), "on_init");
|
||||
|
||||
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
|
||||
{
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
// Loading paramters from Hardware info and .yaml file
|
||||
auto node = this->get_node();
|
||||
node->declare_parameter("min_position", 0.0);
|
||||
node->declare_parameter("max_position", 0.14);
|
||||
node->declare_parameter("min_speed", 0.02);
|
||||
node->declare_parameter("max_speed", 0.15);
|
||||
node->declare_parameter("min_force", 20.0);
|
||||
node->declare_parameter("max_force", 235.0);
|
||||
try
|
||||
{
|
||||
robot_ip_ = info_.hardware_parameters.at("robot_ip");
|
||||
robot_port_ = std::stod(info_.hardware_parameters.at("robot_port"));
|
||||
node->get_parameter("min_position", min_position_);
|
||||
node->get_parameter("max_position", max_position_);
|
||||
node->get_parameter("min_speed", min_speed_);
|
||||
node->get_parameter("max_speed", max_speed_);
|
||||
node->get_parameter("min_force", min_force_);
|
||||
node->get_parameter("max_force", max_force_);
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_FATAL(rclcpp::get_logger("RobotiqGripperHardwareInterface"),
|
||||
"Failed to load parameters: %s", e.what());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
gripper_position_ = std::numeric_limits<double>::quiet_NaN();
|
||||
gripper_velocity_ = std::numeric_limits<double>::quiet_NaN();
|
||||
gripper_position_command_ = std::numeric_limits<double>::quiet_NaN();
|
||||
reactivate_gripper_cmd_ = NO_NEW_CMD_;
|
||||
reactivate_gripper_async_cmd_.store(false);
|
||||
|
||||
const hardware_interface::ComponentInfo& joint = info_.joints[0];
|
||||
|
||||
// There is one command interface: position.
|
||||
if (joint.command_interfaces.size() != 1)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
|
||||
joint.command_interfaces.size());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(),
|
||||
joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
// There are two state interfaces: position and velocity.
|
||||
if (joint.state_interfaces.size() != 2)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
|
||||
joint.state_interfaces.size());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
|
||||
joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(),
|
||||
joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
|
||||
hardware_interface::HW_IF_VELOCITY);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
// Socket Factory -> not added yet
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "on_configure");
|
||||
try
|
||||
{
|
||||
if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS)
|
||||
{
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
|
||||
if (!socket_adapter_->connect(robot_ip_, robot_port_))
|
||||
{
|
||||
RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_ERROR(logger_, "Cannot configure the Robotiq gripper: %s", e.what());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "on_activate");
|
||||
|
||||
// set some default values for joints
|
||||
if (std::isnan(gripper_position_))
|
||||
{
|
||||
gripper_position_ = 0;
|
||||
gripper_velocity_ = 0;
|
||||
gripper_position_command_ = 0;
|
||||
}
|
||||
|
||||
// Activate the gripper.
|
||||
try
|
||||
{
|
||||
socket_adapter_->deactivate();
|
||||
socket_adapter_->activate();
|
||||
|
||||
communication_thread_is_running_.store(true);
|
||||
communication_thread_ = std::thread([this] { this->background_task(); });
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Failed to communicate with the Robotiq gripper: %s", e.what());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(logger_, "Robotiq Gripper successfully activated!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "on_deactivate");
|
||||
|
||||
communication_thread_is_running_.store(false);
|
||||
communication_thread_.join();
|
||||
if (communication_thread_.joinable())
|
||||
{
|
||||
communication_thread_.join();
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
socket_adapter_->deactivate();
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_ERROR(logger_, "Failed to deactivate the Robotiq gripper: %s", e.what());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Robotiq Gripper successfully deactivated!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces()
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "export_state_interfaces");
|
||||
|
||||
std::vector<hardware_interface::StateInterface> state_interfaces;
|
||||
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(
|
||||
info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_
|
||||
)
|
||||
);
|
||||
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(
|
||||
info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_
|
||||
)
|
||||
);
|
||||
|
||||
return state_interfaces;
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterface::export_command_interfaces()
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "export_command_interfaces");
|
||||
|
||||
std::vector<hardware_interface::CommandInterface> command_interfaces;
|
||||
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_
|
||||
)
|
||||
);
|
||||
|
||||
// Handling potential uninitialized parameter 'gripper_speed_multiplier'
|
||||
double gripper_speed_multiplier = 1.0;
|
||||
if (info_.hardware_parameters.find("gripper_speed_multiplier") != info_.hardware_parameters.end()) {
|
||||
gripper_speed_multiplier = std::stod(info_.hardware_parameters["gripper_speed_multiplier"]);
|
||||
}
|
||||
gripper_speed_ = gripper_speed_multiplier;
|
||||
|
||||
// Handling potential uninitialized parameter 'gripper_force_multiplier'
|
||||
double gripper_force_multiplier = 1.0;
|
||||
if (info_.hardware_parameters.find("gripper_force_multiplier") != info_.hardware_parameters.end()) {
|
||||
gripper_force_multiplier = std::stod(info_.hardware_parameters["gripper_force_multiplier"]);
|
||||
}
|
||||
gripper_force_ = gripper_force_multiplier;
|
||||
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_));
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_));
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_));
|
||||
|
||||
return command_interfaces;
|
||||
}
|
||||
|
||||
hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& time, const rclcpp::Duration& /*period*/)
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "Reading state from the gripper");
|
||||
|
||||
// Fetch current position and other states from the hardware
|
||||
try {
|
||||
int raw_position = socket_adapter_->getGripperPosition(); // This should be an existing method in your adapter
|
||||
double new_position = convertRawToPosition(raw_position);
|
||||
rclcpp::Time current_time = time;
|
||||
|
||||
if (!std::isnan(gripper_position_)) {
|
||||
double time_difference = (current_time - last_update_time_).seconds(); // Calculate time difference in seconds
|
||||
if (time_difference > 0) {
|
||||
gripper_velocity_ = (new_position - gripper_position_) / time_difference; // Calculate velocity
|
||||
}
|
||||
}
|
||||
|
||||
gripper_position_ = new_position; // Update current position
|
||||
last_update_time_ = current_time; // Update last update timestamp
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(logger_, "Failed to read from Robotiq gripper: %s", e.what());
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
// Handle reactivation command if set
|
||||
if (!std::isnan(reactivate_gripper_cmd_) && reactivate_gripper_cmd_ != NO_NEW_CMD_) {
|
||||
RCLCPP_INFO(logger_, "Sending gripper reactivation request.");
|
||||
reactivate_gripper_async_cmd_.store(true);
|
||||
reactivate_gripper_cmd_ = NO_NEW_CMD_; // Reset command
|
||||
}
|
||||
|
||||
// Process asynchronous reactivation response if available
|
||||
if (reactivate_gripper_async_response_.load().has_value()) {
|
||||
reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value();
|
||||
reactivate_gripper_async_response_.store(std::nullopt); // Reset response
|
||||
}
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
|
||||
{
|
||||
try {
|
||||
// Use the conversion methods to scale the command values to the range expected by the hardware
|
||||
int scaled_position = convertPositionToRaw(gripper_position_command_);
|
||||
int scaled_speed = convertSpeedToRaw(gripper_speed_);
|
||||
int scaled_force = convertForceToRaw(gripper_force_);
|
||||
|
||||
// Store the scaled values into atomic variables
|
||||
write_command_.store(scaled_position);
|
||||
write_speed_.store(scaled_speed);
|
||||
write_force_.store(scaled_force);
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(logger_, "Failed to write to Robotiq gripper: %s", e.what());
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
void RobotiqGripperHardwareInterface::background_task()
|
||||
{
|
||||
while (communication_thread_is_running_.load())
|
||||
{
|
||||
try
|
||||
{
|
||||
// Re-activate the gripper
|
||||
// This can be used, for example, to re-run the auto-calibration.
|
||||
if (reactivate_gripper_async_cmd_.load())
|
||||
{
|
||||
socket_adapter_->deactivate();
|
||||
socket_adapter_->activate();
|
||||
reactivate_gripper_async_cmd_.store(false);
|
||||
reactivate_gripper_async_response_.store(true);
|
||||
}
|
||||
|
||||
// Write the latest command to the gripper.
|
||||
int scaled_position = write_command_.load();
|
||||
int scaled_speed = write_speed_.load();
|
||||
int scaled_force = write_force_.load();
|
||||
|
||||
if (!socket_adapter_->move(scaled_position, scaled_speed, scaled_force)) {
|
||||
throw std::runtime_error("Failed to send move command to Robotiq gripper.");
|
||||
}
|
||||
|
||||
// Read the state of the gripper.
|
||||
int raw_position = socket_adapter_->getGripperPosition();
|
||||
gripper_current_state_.store(convertRawToPosition(raw_position));
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(gripperCommsLoopPeriod); // Sleep for a predefined period
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Conversion methods
|
||||
double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
|
||||
if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
|
||||
throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
|
||||
}
|
||||
double scaled_position = min_position_ + (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
|
||||
return std::max(min_position_, std::min(scaled_position, max_position_));
|
||||
}
|
||||
|
||||
uint8_t RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
|
||||
if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
|
||||
throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
|
||||
}
|
||||
double scaled = (position - min_position_) / (max_position_ - min_position_) * 255.0;
|
||||
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
|
||||
}
|
||||
|
||||
uint8_t RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
|
||||
if (std::isnan(min_speed_) || std::isnan(max_speed_) || min_speed_ >= max_speed_) {
|
||||
throw std::runtime_error("Invalid gripper speed limits: min_speed_ or max_speed_ are not configured correctly.");
|
||||
}
|
||||
double scaled = (speed - min_speed_) / (max_speed_ - min_speed_) * 255.0;
|
||||
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
|
||||
}
|
||||
|
||||
uint8_t RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
|
||||
if (std::isnan(min_force_) || std::isnan(max_force_) || min_force_ >= max_force_) {
|
||||
throw std::runtime_error("Invalid gripper force limits: min_force_ or max_force_ are not configured correctly.");
|
||||
}
|
||||
double scaled = (force - min_force_) / (max_force_ - min_force_) * 255.0;
|
||||
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
|
||||
}
|
||||
|
||||
} // namespace robotiq_driver
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(robotiq_2f_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)
|
||||
@ -1,69 +0,0 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include "RobotiqGripperHardwareInterface.hpp"
|
||||
|
||||
class TestableRobotiqGripperHardwareInterface : public robotiq_driver::RobotiqGripperHardwareInterface {
|
||||
public:
|
||||
using robotiq_driver::RobotiqGripperHardwareInterface::convertPositionToRaw;
|
||||
using robotiq_driver::RobotiqGripperHardwareInterface::convertRawToPosition;
|
||||
using robotiq_driver::RobotiqGripperHardwareInterface::convertSpeedToRaw;
|
||||
using robotiq_driver::RobotiqGripperHardwareInterface::convertForceToRaw;
|
||||
};
|
||||
|
||||
class RobotiqGripperConversionTests : public ::testing::Test {
|
||||
protected:
|
||||
TestableRobotiqGripperHardwareInterface gripper_interface;
|
||||
|
||||
void SetUp() override {
|
||||
gripper_interface.min_position_ = 0.0;
|
||||
gripper_interface.max_position_ = 0.14;
|
||||
gripper_interface.min_speed_ = 0.02;
|
||||
gripper_interface.max_speed_ = 0.15;
|
||||
gripper_interface.min_force_ = 20.0;
|
||||
gripper_interface.max_force_ = 235.0;
|
||||
}
|
||||
};
|
||||
|
||||
// Test for convertPositionToRaw
|
||||
TEST_F(RobotiqGripperConversionTests, PositionToRawValidInput) {
|
||||
EXPECT_EQ(gripper_interface.convertPositionToRaw(0.07), 128);
|
||||
}
|
||||
|
||||
TEST_F(RobotiqGripperConversionTests, PositionToRawBoundaryInput) {
|
||||
EXPECT_EQ(gripper_interface.convertPositionToRaw(0.0), 0);
|
||||
EXPECT_EQ(gripper_interface.convertPositionToRaw(0.14), 255);
|
||||
}
|
||||
|
||||
// Test for convertRawToPosition
|
||||
TEST_F(RobotiqGripperConversionTests, RawToPositionValidInput) {
|
||||
ASSERT_NEAR(gripper_interface.convertRawToPosition(128), 0.07, 0.01);
|
||||
}
|
||||
|
||||
TEST_F(RobotiqGripperConversionTests, RawToPositionBoundaryInput) {
|
||||
ASSERT_NEAR(gripper_interface.convertRawToPosition(0), 0.0, 0.01);
|
||||
ASSERT_NEAR(gripper_interface.convertRawToPosition(255), 0.14, 0.01);
|
||||
}
|
||||
|
||||
// Test for convertSpeedToRaw
|
||||
TEST_F(RobotiqGripperConversionTests, SpeedToRawValidInput) {
|
||||
EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.085), 128);
|
||||
}
|
||||
|
||||
TEST_F(RobotiqGripperConversionTests, SpeedToRawBoundaryInput) {
|
||||
EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.02), 0);
|
||||
EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.15), 255);
|
||||
}
|
||||
|
||||
// Test for convertForceToRaw
|
||||
TEST_F(RobotiqGripperConversionTests, ForceToRawValidInput) {
|
||||
EXPECT_EQ(gripper_interface.convertForceToRaw(127.5), 128);
|
||||
}
|
||||
|
||||
TEST_F(RobotiqGripperConversionTests, ForceToRawBoundaryInput) {
|
||||
EXPECT_EQ(gripper_interface.convertForceToRaw(20.0), 0);
|
||||
EXPECT_EQ(gripper_interface.convertForceToRaw(235.0), 255);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
52
src/servo_keyboard/CMakeLists.txt
Normal file
52
src/servo_keyboard/CMakeLists.txt
Normal file
@ -0,0 +1,52 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(servo_keyboard)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(control_msgs REQUIRED)
|
||||
find_package(controller_manager_msgs REQUIRED)
|
||||
|
||||
add_executable(servo_keyboard_input src/servo_keyboard_input.cpp)
|
||||
target_include_directories(servo_keyboard_input PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(servo_keyboard_input PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
|
||||
# Include directories
|
||||
target_include_directories(servo_keyboard_input PRIVATE
|
||||
${rclcpp_INCLUDE_DIRS}
|
||||
${geometry_msgs_INCLUDE_DIRS}
|
||||
${control_msgs_INCLUDE_DIRS}
|
||||
${controller_manager_msgs_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# Link the executable with required libraries
|
||||
ament_target_dependencies(servo_keyboard_input
|
||||
rclcpp
|
||||
geometry_msgs
|
||||
control_msgs
|
||||
controller_manager_msgs
|
||||
)
|
||||
|
||||
install(TARGETS servo_keyboard_input
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
202
src/servo_keyboard/LICENSE
Normal file
202
src/servo_keyboard/LICENSE
Normal file
@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
@ -1,19 +1,21 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>robotiq_2f_interface</name>
|
||||
<name>servo_keyboard</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
<maintainer email="ligerfotis@gmail.com">Fotios Lyegrakis</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<!-- Dependencies -->
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>lifecycle_msgs</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>control_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<build_depend>controller_manager_msgs</build_depend>
|
||||
<exec_depend>controller_manager_msgs</exec_depend>
|
||||
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
389
src/servo_keyboard/src/servo_keyboard_input.cpp
Normal file
389
src/servo_keyboard/src/servo_keyboard_input.cpp
Normal file
@ -0,0 +1,389 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2021, PickNik LLC
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of PickNik LLC nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Title : servo_keyboard_input.cpp
|
||||
* Project : moveit2_tutorials
|
||||
* Created : 05/31/2021
|
||||
* Author : Adam Pettinger
|
||||
*/
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <geometry_msgs/msg/twist_stamped.hpp>
|
||||
#include <control_msgs/msg/joint_jog.hpp>
|
||||
#include <controller_manager_msgs/srv/switch_controller.hpp> // Add necessary includes
|
||||
#include <std_msgs/msg/float64_multi_array.hpp> // Add necessary includes
|
||||
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
// Define used keys
|
||||
#define KEYCODE_LEFT 0x44
|
||||
#define KEYCODE_RIGHT 0x43
|
||||
#define KEYCODE_UP 0x41
|
||||
#define KEYCODE_DOWN 0x42
|
||||
#define KEYCODE_PERIOD 0x2E
|
||||
#define KEYCODE_SEMICOLON 0x3B
|
||||
#define KEYCODE_E 0x65
|
||||
#define KEYCODE_W 0x77
|
||||
#define KEYCODE_1 0x31
|
||||
#define KEYCODE_2 0x32
|
||||
#define KEYCODE_3 0x33
|
||||
#define KEYCODE_4 0x34
|
||||
#define KEYCODE_5 0x35
|
||||
#define KEYCODE_6 0x36
|
||||
#define KEYCODE_R 0x72
|
||||
#define KEYCODE_Q 0x71
|
||||
#define KEYCODE_PLUS 0x2B // Keycode for the plus sign (+)
|
||||
#define KEYCODE_MINUS 0x2D // Keycode for the minus sign (-)
|
||||
#define KEYCODE_GRIPPER 0x67 // Keycode for the gripper control button (g)
|
||||
|
||||
// Some constants used in the Servo Teleop demo
|
||||
const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
|
||||
const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
|
||||
const std::string GRIPPER_TOPIC = "/forward_gripper_position_controller/commands";
|
||||
const size_t ROS_QUEUE_SIZE = 10;
|
||||
const std::string EEF_FRAME_ID = "world";
|
||||
const std::string BASE_FRAME_ID = "tool0";
|
||||
|
||||
// A class for reading the key inputs from the terminal
|
||||
class KeyboardReader
|
||||
{
|
||||
public:
|
||||
KeyboardReader() : kfd(0)
|
||||
{
|
||||
// get the console in raw mode
|
||||
tcgetattr(kfd, &cooked);
|
||||
struct termios raw;
|
||||
memcpy(&raw, &cooked, sizeof(struct termios));
|
||||
raw.c_lflag &= ~(ICANON | ECHO);
|
||||
// Setting a new line, then end of file
|
||||
raw.c_cc[VEOL] = 1;
|
||||
raw.c_cc[VEOF] = 2;
|
||||
tcsetattr(kfd, TCSANOW, &raw);
|
||||
}
|
||||
void readOne(char* c)
|
||||
{
|
||||
int rc = read(kfd, c, 1);
|
||||
if (rc < 0)
|
||||
{
|
||||
throw std::runtime_error("read failed");
|
||||
}
|
||||
}
|
||||
void shutdown()
|
||||
{
|
||||
tcsetattr(kfd, TCSANOW, &cooked);
|
||||
}
|
||||
|
||||
private:
|
||||
int kfd;
|
||||
struct termios cooked;
|
||||
};
|
||||
|
||||
// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller
|
||||
class KeyboardServo
|
||||
{
|
||||
public:
|
||||
KeyboardServo();
|
||||
int keyLoop();
|
||||
|
||||
private:
|
||||
void spin();
|
||||
|
||||
void handlePlusPress();
|
||||
void handleMinusPress();
|
||||
|
||||
void publishGripperCommand(double finger_joint_angle);
|
||||
|
||||
rclcpp::Node::SharedPtr nh_;
|
||||
|
||||
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
|
||||
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_pub_;
|
||||
|
||||
std::string frame_to_publish_;
|
||||
double joint_vel_cmd_;
|
||||
double gripper_speed_multiplier_;
|
||||
double last_finger_joint_angle_;
|
||||
double gripper_lower_limit_;
|
||||
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");
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
KeyboardReader input;
|
||||
|
||||
void KeyboardServo::publishGripperCommand(double finger_joint_angle)
|
||||
{
|
||||
auto msg = std::make_unique<std_msgs::msg::Float64MultiArray>();
|
||||
msg->data.push_back(finger_joint_angle);
|
||||
gripper_cmd_pub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
void quit(int sig)
|
||||
{
|
||||
(void)sig;
|
||||
input.shutdown();
|
||||
rclcpp::shutdown();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
KeyboardServo keyboard_servo;
|
||||
|
||||
signal(SIGINT, quit);
|
||||
|
||||
int rc = keyboard_servo.keyLoop();
|
||||
input.shutdown();
|
||||
rclcpp::shutdown();
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
void KeyboardServo::spin()
|
||||
{
|
||||
while (rclcpp::ok())
|
||||
{
|
||||
rclcpp::spin_some(nh_);
|
||||
}
|
||||
}
|
||||
|
||||
void KeyboardServo::handlePlusPress()
|
||||
{
|
||||
// Calculate the new finger joint angle
|
||||
double delta_angle = 1.0 * gripper_speed_multiplier_;
|
||||
double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle;
|
||||
// printf("New finger joint angle: %f\n", new_finger_joint_angle);
|
||||
// Check if the new angle is within the limits
|
||||
if (new_finger_joint_angle <= gripper_upper_limit_)
|
||||
{
|
||||
// Update the finger joint angle
|
||||
last_finger_joint_angle_ = new_finger_joint_angle;
|
||||
// printf("New finger joint angle: %f\n", last_finger_joint_angle_);
|
||||
// Publish the gripper command with the new angle
|
||||
publishGripperCommand(last_finger_joint_angle_);
|
||||
}
|
||||
}
|
||||
|
||||
void KeyboardServo::handleMinusPress()
|
||||
{
|
||||
// Calculate the new finger joint angle
|
||||
double delta_angle = -1.0 * gripper_speed_multiplier_;
|
||||
double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle;
|
||||
|
||||
// Check if the new angle is within the limits
|
||||
if (new_finger_joint_angle >= gripper_lower_limit_)
|
||||
{
|
||||
// Update the finger joint angle
|
||||
last_finger_joint_angle_ = new_finger_joint_angle;
|
||||
|
||||
// Publish the gripper command with the new angle
|
||||
publishGripperCommand(last_finger_joint_angle_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int KeyboardServo::keyLoop()
|
||||
{
|
||||
char c;
|
||||
bool publish_twist = false;
|
||||
bool publish_joint = false;
|
||||
|
||||
std::thread{ std::bind(&KeyboardServo::spin, this) }.detach();
|
||||
|
||||
puts("Reading from keyboard");
|
||||
puts("---------------------------");
|
||||
puts("Use arrow keys and the '.' and ';' keys to Cartesian jog");
|
||||
puts("Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame");
|
||||
puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging.");
|
||||
puts("Use '+' to open the gripper, '-' to close it.");
|
||||
puts("'Q' to quit.");
|
||||
|
||||
|
||||
for (;;)
|
||||
{
|
||||
// get the next event from the keyboard
|
||||
try
|
||||
{
|
||||
input.readOne(&c);
|
||||
}
|
||||
catch (const std::runtime_error&)
|
||||
{
|
||||
perror("read():");
|
||||
return -1;
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c);
|
||||
|
||||
// // Create the messages we might publish
|
||||
auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
|
||||
auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
|
||||
|
||||
// Use read key-press
|
||||
switch (c)
|
||||
{
|
||||
case KEYCODE_LEFT:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "LEFT");
|
||||
twist_msg->twist.linear.y = -1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_RIGHT:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "RIGHT");
|
||||
twist_msg->twist.linear.y = 1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_UP:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "UP");
|
||||
twist_msg->twist.linear.x = 1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_DOWN:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "DOWN");
|
||||
twist_msg->twist.linear.x = -1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_PERIOD:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "PERIOD");
|
||||
twist_msg->twist.linear.z = -1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_SEMICOLON:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON");
|
||||
twist_msg->twist.linear.z = 1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_E:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "E");
|
||||
frame_to_publish_ = EEF_FRAME_ID;
|
||||
break;
|
||||
case KEYCODE_W:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "W");
|
||||
frame_to_publish_ = BASE_FRAME_ID;
|
||||
break;
|
||||
case KEYCODE_1:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "1");
|
||||
joint_msg->joint_names.push_back("shoulder_lift_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_2:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "2");
|
||||
joint_msg->joint_names.push_back("shoulder_pan_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_3:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "3");
|
||||
joint_msg->joint_names.push_back("elbow_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_4:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "4");
|
||||
joint_msg->joint_names.push_back("wrist_1_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_5:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "5");
|
||||
joint_msg->joint_names.push_back("wrist_2_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_6:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "6");
|
||||
joint_msg->joint_names.push_back("wrist_3_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
// case KEYCODE_7:
|
||||
// RCLCPP_DEBUG(nh_->get_logger(), "7");
|
||||
// joint_msg->joint_names.push_back("finger_joint");
|
||||
// joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
// publish_joint = true;
|
||||
// break;
|
||||
case KEYCODE_R:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "R");
|
||||
joint_vel_cmd_ *= -1;
|
||||
break;
|
||||
case KEYCODE_Q:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "quit");
|
||||
return 0;
|
||||
// Add cases for other keys as needed
|
||||
case KEYCODE_PLUS:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "PLUS");
|
||||
handlePlusPress();
|
||||
break;
|
||||
case KEYCODE_MINUS:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "MINUS");
|
||||
handleMinusPress();
|
||||
break;
|
||||
}
|
||||
|
||||
// If a key requiring a publish was pressed, publish the message now
|
||||
if (publish_twist)
|
||||
{
|
||||
twist_msg->header.stamp = nh_->now();
|
||||
twist_msg->header.frame_id = frame_to_publish_;
|
||||
twist_pub_->publish(std::move(twist_msg));
|
||||
publish_twist = false;
|
||||
}
|
||||
else if (publish_joint)
|
||||
{
|
||||
joint_msg->header.stamp = nh_->now();
|
||||
joint_msg->header.frame_id = BASE_FRAME_ID;
|
||||
joint_pub_->publish(std::move(joint_msg));
|
||||
publish_joint = false;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,44 @@
|
||||
kinematics:
|
||||
shoulder:
|
||||
x: 0
|
||||
y: 0
|
||||
z: 0.1519161231793138
|
||||
roll: -0
|
||||
pitch: 0
|
||||
yaw: -8.3688888041777445e-08
|
||||
upper_arm:
|
||||
x: -9.8255319984929057e-05
|
||||
y: 0
|
||||
z: 0
|
||||
roll: 1.5713473529266058
|
||||
pitch: 0
|
||||
yaw: 4.5372573280303068e-06
|
||||
forearm:
|
||||
x: -0.24361145101605494
|
||||
y: 0
|
||||
z: 0
|
||||
roll: 3.138959939545904
|
||||
pitch: 3.141417818890341
|
||||
yaw: 3.1415861917625292
|
||||
wrist_1:
|
||||
x: -0.21328351386382516
|
||||
y: -0.00058695912107010507
|
||||
z: 0.13086135281159214
|
||||
roll: 0.0044853210843803608
|
||||
pitch: 0.00071590512460444685
|
||||
yaw: -2.0286176909971606e-06
|
||||
wrist_2:
|
||||
x: 9.0086536104922084e-05
|
||||
y: -0.085314349728116412
|
||||
z: -6.1195228207677147e-05
|
||||
roll: 1.5715136178239859
|
||||
pitch: 0
|
||||
yaw: 1.3915445690113049e-06
|
||||
wrist_3:
|
||||
x: -0.00014076969524819022
|
||||
y: 0.092309359181826575
|
||||
z: -0.00011628039579221433
|
||||
roll: 1.5695366459205147
|
||||
pitch: 3.1415926535897931
|
||||
yaw: 3.1415925648453848
|
||||
hash: calib_2535041911463403862
|
||||
@ -6,7 +6,8 @@ from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution
|
||||
|
||||
# from moveit_configs_utils import MoveItConfigsBuilder
|
||||
# from launch_param_builder import ParameterBuilder
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
# Initialize Arguments
|
||||
@ -170,6 +171,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
|
||||
initial_joint_controllers = PathJoinSubstitution(
|
||||
[FindPackageShare(description_package), "config", controllers_file]
|
||||
)
|
||||
@ -275,6 +277,38 @@ def launch_setup(context, *args, **kwargs):
|
||||
},
|
||||
],
|
||||
)
|
||||
############################################################################################################
|
||||
# # Get parameters for the Servo node
|
||||
# servo_params = (
|
||||
# ParameterBuilder("ur_robotiq_servo")
|
||||
# .yaml(
|
||||
# parameter_namespace="moveit_servo",
|
||||
# file_path="config/ur_robotiq_simulated_config.yaml",
|
||||
# )
|
||||
# .to_dict()
|
||||
# )
|
||||
# print(servo_params)
|
||||
#
|
||||
# # A node to publish world -> panda_link0 transform
|
||||
# static_tf = Node(
|
||||
# package="tf2_ros",
|
||||
# executable="static_transform_publisher",
|
||||
# name="static_transform_publisher",
|
||||
# output="log",
|
||||
# arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
|
||||
# )
|
||||
#
|
||||
# # The servo cpp interface demo
|
||||
# # Creates the Servo node and publishes commands to it
|
||||
# servo_node = Node(
|
||||
# package="moveit2_tutorials",
|
||||
# executable="servo_cpp_interface_demo",
|
||||
# output="screen",
|
||||
# parameters=[
|
||||
# servo_params,
|
||||
# robot_description,
|
||||
# ],
|
||||
# )
|
||||
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
@ -353,6 +387,8 @@ def launch_setup(context, *args, **kwargs):
|
||||
ur_control_node,
|
||||
dashboard_client_node,
|
||||
controller_stopper_node,
|
||||
# static_tf,
|
||||
# servo_node,
|
||||
robot_state_publisher_node,
|
||||
rviz_node,
|
||||
initial_joint_controller_spawner_stopped,
|
||||
|
||||
@ -1,9 +1,9 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from ur_robotiq.urdf.xacro | -->
|
||||
<!-- | This document was autogenerated by xacro from src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="ur_manipulator">
|
||||
<robot name="ur">
|
||||
<!--
|
||||
Base UR robot series xacro macro.
|
||||
|
||||
@ -59,7 +59,7 @@
|
||||
this location. Nor can they rely on the existence of the macro.
|
||||
-->
|
||||
<link name="world"/>
|
||||
<ros2_control name="ur_manipulator" type="system">
|
||||
<ros2_control name="ur" type="system">
|
||||
<hardware>
|
||||
<plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
|
||||
<param name="robot_ip">0.0.0.0</param>
|
||||
@ -99,7 +99,7 @@
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<!-- initial position for the FakeSystem and simulation -->
|
||||
<param name="initial_value">0.0</param>
|
||||
<param name="initial_value">1.4</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
@ -131,7 +131,7 @@
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<!-- initial position for the FakeSystem and simulation -->
|
||||
<param name="initial_value">0.0</param>
|
||||
<param name="initial_value">1.57</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
@ -163,7 +163,7 @@
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<!-- initial position for the FakeSystem and simulation -->
|
||||
<param name="initial_value">0.0</param>
|
||||
<param name="initial_value">-1.57</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
@ -591,7 +591,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -600,7 +600,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -618,7 +618,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
@ -627,7 +627,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -640,7 +640,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -649,7 +649,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -662,7 +662,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -671,7 +671,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -704,7 +704,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -713,7 +713,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -726,7 +726,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
@ -735,7 +735,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -748,7 +748,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -757,7 +757,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -770,7 +770,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -779,7 +779,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -812,7 +812,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -821,7 +821,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
@ -1,39 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="ur_robotiq_adapter">
|
||||
<xacro:macro name="ur_to_robotiq" params="prefix parent child rotation:=^|${0.0}">
|
||||
|
||||
<joint name="${prefix}ur_to_robotiq_joint" type="fixed">
|
||||
<!-- The parent link must be read from the robot model it is attached to. -->
|
||||
<parent link="${parent}"/>
|
||||
<child link="${prefix}ur_to_robotiq_link"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 ${rotation}"/>
|
||||
</joint>
|
||||
|
||||
<link name="${prefix}ur_to_robotiq_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_description/meshes/visual/2f_85/ur_to_robotiq_adapter.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_description/meshes/collision/2f_85/ur_to_robotiq_adapter.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01" />
|
||||
<origin xyz="0 0 0" />
|
||||
<inertia ixx="0.000044" ixy="0.0" ixz="0.0" iyy="0.000027" iyz="0.0" izz="0.000027" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}gripper_side_joint" type="fixed">
|
||||
<parent link="${prefix}ur_to_robotiq_link"/>
|
||||
<child link="${child}"/>
|
||||
<!-- <origin xyz="0 0 0.011" rpy="0 ${-pi/2} ${pi/2}"/> -->
|
||||
<origin xyz="0 0 0.011" rpy="0 0 0"/>
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
@ -1,7 +1,8 @@
|
||||
joy_node:
|
||||
ros__parameters:
|
||||
device_id: 0
|
||||
device_name: "PS5 Controller"
|
||||
# device_name: "PS5 Controller"
|
||||
device_name: "Keyboard"
|
||||
deadzone: 0.5
|
||||
autorepeat_rate: 20.0
|
||||
sticky_buttons: false
|
||||
|
||||
6
src/ur_robotiq_servo/config/kb-params.yaml
Normal file
6
src/ur_robotiq_servo/config/kb-params.yaml
Normal file
@ -0,0 +1,6 @@
|
||||
keyboard_servo_node:
|
||||
ros__parameters:
|
||||
linear_speed_multiplier: 0.2
|
||||
gripper_speed_multiplier: 0.02
|
||||
gripper_lower_limit: 0.0
|
||||
gripper_upper_limit: 0.70
|
||||
46
src/ur_robotiq_servo/launch/kb_servo.launch.py
Normal file
46
src/ur_robotiq_servo/launch/kb_servo.launch.py
Normal file
@ -0,0 +1,46 @@
|
||||
import os
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
import ament_index_python.packages
|
||||
from launch_param_builder import ParameterBuilder
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
|
||||
def generate_launch_description():
|
||||
config_directory = os.path.join(
|
||||
ament_index_python.packages.get_package_share_directory('ur_robotiq_servo'),
|
||||
'config')
|
||||
joy_params = os.path.join(config_directory, 'joy-params.yaml')
|
||||
ps5_params = os.path.join(config_directory, 'ps5-params.yaml')
|
||||
|
||||
moveit_config = (
|
||||
MoveItConfigsBuilder("moveit_resources_panda")
|
||||
.robot_description(file_path="config/panda.urdf.xacro")
|
||||
.to_moveit_configs()
|
||||
)
|
||||
# Get parameters for the Servo node
|
||||
servo_params = (
|
||||
ParameterBuilder("moveit_servo")
|
||||
.yaml(
|
||||
parameter_namespace="moveit_servo",
|
||||
file_path="config/panda_simulated_config.yaml",
|
||||
)
|
||||
.to_dict()
|
||||
)
|
||||
|
||||
# The servo cpp interface demo
|
||||
# Creates the Servo node and publishes commands to it
|
||||
servo_node = Node(
|
||||
package="moveit2_tutorials",
|
||||
executable="servo_cpp_interface_demo",
|
||||
output="screen",
|
||||
parameters=[
|
||||
servo_params,
|
||||
moveit_config.robot_description,
|
||||
moveit_config.robot_description_semantic,
|
||||
],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
servo_node
|
||||
])
|
||||
@ -14,6 +14,7 @@
|
||||
<depend>std_srvs</depend>
|
||||
<depend>joy</depend>
|
||||
<depend>moveit_servo</depend>
|
||||
<depend>keyboard</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
|
||||
196
src/ur_robotiq_servo/servo_keyboard_input.py
Normal file
196
src/ur_robotiq_servo/servo_keyboard_input.py
Normal file
@ -0,0 +1,196 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import tty
|
||||
import termios
|
||||
import threading
|
||||
import signal
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import TwistStamped
|
||||
from control_msgs.msg import JointJog
|
||||
|
||||
# Define key codes
|
||||
KEYCODE_RIGHT = 0x43
|
||||
KEYCODE_LEFT = 0x44
|
||||
KEYCODE_UP = 0x41
|
||||
KEYCODE_DOWN = 0x42
|
||||
KEYCODE_PERIOD = 0x2E
|
||||
KEYCODE_SEMICOLON = 0x3B
|
||||
KEYCODE_1 = 0x31
|
||||
KEYCODE_2 = 0x32
|
||||
KEYCODE_3 = 0x33
|
||||
KEYCODE_4 = 0x34
|
||||
KEYCODE_5 = 0x35
|
||||
KEYCODE_6 = 0x36
|
||||
KEYCODE_7 = 0x37
|
||||
KEYCODE_Q = 0x71
|
||||
KEYCODE_R = 0x72
|
||||
KEYCODE_J = 0x6A
|
||||
KEYCODE_T = 0x74
|
||||
KEYCODE_W = 0x77
|
||||
KEYCODE_E = 0x65
|
||||
|
||||
# Constants used in the Servo Teleop demo
|
||||
TWIST_TOPIC = "/servo_node/delta_twist_cmds"
|
||||
JOINT_TOPIC = "/servo_node/delta_joint_cmds"
|
||||
ROS_QUEUE_SIZE = 10
|
||||
PLANNING_FRAME_ID = "world"
|
||||
EE_FRAME_ID = "tool0"
|
||||
|
||||
|
||||
class KeyboardReader:
|
||||
def __init__(self):
|
||||
self.file_descriptor = sys.stdin.fileno()
|
||||
self.old_settings = termios.tcgetattr(self.file_descriptor)
|
||||
tty.setraw(self.file_descriptor)
|
||||
|
||||
def read_one(self):
|
||||
return sys.stdin.read(1)
|
||||
|
||||
def shutdown(self):
|
||||
termios.tcsetattr(self.file_descriptor, termios.TCSADRAIN, self.old_settings)
|
||||
|
||||
|
||||
class KeyboardServo:
|
||||
def __init__(self):
|
||||
self.joint_vel_cmd = 1.0
|
||||
self.command_frame_id = PLANNING_FRAME_ID
|
||||
self.node = rclpy.create_node("servo_keyboard_input")
|
||||
self.twist_pub = self.node.create_publisher(TwistStamped, TWIST_TOPIC, ROS_QUEUE_SIZE)
|
||||
self.joint_pub = self.node.create_publisher(JointJog, JOINT_TOPIC, ROS_QUEUE_SIZE)
|
||||
self.switch_input = self.node.create_client(ServoCommandType, "servo_node/switch_command_type")
|
||||
self.request = ServoCommandType.Request()
|
||||
|
||||
def spin(self):
|
||||
rclpy.spin(self.node)
|
||||
|
||||
def key_loop(self):
|
||||
publish_twist = False
|
||||
publish_joint = False
|
||||
print("Reading from keyboard")
|
||||
print("---------------------------")
|
||||
print("All commands are in the planning frame")
|
||||
print("Use arrow keys and the '.' and ';' keys to Cartesian jog")
|
||||
print("Use 1|2|3|4|5|6|7 keys to joint jog. 'r' to reverse the direction of jogging.")
|
||||
print("Use 'j' to select joint jog.")
|
||||
print("Use 't' to select twist")
|
||||
print("Use 'w' and 'e' to switch between sending command in planning frame or end effector frame")
|
||||
print("'Q' to quit.")
|
||||
|
||||
while True:
|
||||
c = input()
|
||||
|
||||
twist_msg = TwistStamped()
|
||||
joint_msg = JointJog()
|
||||
joint_msg.joint_names = [
|
||||
"shoulder_lift_joint",
|
||||
"shoulder_pan_joint",
|
||||
"elbow_joint",
|
||||
"wrist_1_joint",
|
||||
"wrist_2_joint",
|
||||
"wrist_2_joint",
|
||||
"finger_joint",
|
||||
]
|
||||
joint_msg.velocities = [0.0] * 7
|
||||
|
||||
# Use read key-press
|
||||
if c == '\x1b': # ANSI escape sequence
|
||||
c = input()
|
||||
if c == '[':
|
||||
c = input()
|
||||
if c == 'A':
|
||||
twist_msg.twist.linear.x = 0.5 # UP
|
||||
publish_twist = True
|
||||
elif c == 'B':
|
||||
twist_msg.twist.linear.x = -0.5 # DOWN
|
||||
publish_twist = True
|
||||
elif c == 'C':
|
||||
twist_msg.twist.linear.y = 0.5 # RIGHT
|
||||
publish_twist = True
|
||||
elif c == 'D':
|
||||
twist_msg.twist.linear.y = -0.5 # LEFT
|
||||
publish_twist = True
|
||||
elif ord(c) == KEYCODE_PERIOD:
|
||||
twist_msg.twist.linear.z = -0.5 # '.'
|
||||
publish_twist = True
|
||||
elif ord(c) == KEYCODE_SEMICOLON:
|
||||
twist_msg.twist.linear.z = 0.5 # ';'
|
||||
publish_twist = True
|
||||
elif ord(c) == KEYCODE_1:
|
||||
joint_msg.velocities[0] = self.joint_vel_cmd # '1'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_2:
|
||||
joint_msg.velocities[1] = self.joint_vel_cmd # '2'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_3:
|
||||
joint_msg.velocities[2] = self.joint_vel_cmd # '3'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_4:
|
||||
joint_msg.velocities[3] = self.joint_vel_cmd # '4'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_5:
|
||||
joint_msg.velocities[4] = self.joint_vel_cmd # '5'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_6:
|
||||
joint_msg.velocities[5] = self.joint_vel_cmd # '6'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_7:
|
||||
joint_msg.velocities[6] = self.joint_vel_cmd # '7'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_R:
|
||||
self.joint_vel_cmd *= -1 # 'r'
|
||||
elif ord(c) == KEYCODE_J:
|
||||
self.request.command_type = ServoCommandType.Request.JOINT_JOG # 'j'
|
||||
if self.switch_input.wait_for_service(timeout_sec=1):
|
||||
result = self.switch_input.call(self.request)
|
||||
if result.success:
|
||||
print("Switched to input type: JointJog")
|
||||
else:
|
||||
print("Could not switch input to: JointJog")
|
||||
elif ord(c) == KEYCODE_T:
|
||||
self.request.command_type = ServoCommandType.Request.TWIST # 't'
|
||||
if self.switch_input.wait_for_service(timeout_sec=1):
|
||||
result = self.switch_input.call(self.request)
|
||||
if result.success:
|
||||
print("Switched to input type: Twist")
|
||||
else:
|
||||
print("Could not switch input to: Twist")
|
||||
elif ord(c) == KEYCODE_W:
|
||||
print(f"Command frame set to: {PLANNING_FRAME_ID}") # 'w'
|
||||
self.command_frame_id = PLANNING_FRAME_ID
|
||||
elif ord(c) == KEYCODE_E:
|
||||
print(f"Command frame set to: {EE_FRAME_ID}") # 'e'
|
||||
self.command_frame_id = EE_FRAME_ID
|
||||
elif ord(c) == KEYCODE_Q:
|
||||
print("Quit") # 'Q'
|
||||
return 0
|
||||
|
||||
# If a key requiring a publish was pressed, publish the message now
|
||||
if publish_twist:
|
||||
twist_msg.header.stamp = self.node.get_clock().now().to_msg()
|
||||
twist_msg.header.frame_id = self.command_frame_id
|
||||
self.twist_pub.publish(twist_msg)
|
||||
publish_twist = False
|
||||
elif publish_joint:
|
||||
joint_msg.header.stamp = self.node.get_clock().now().to_msg()
|
||||
joint_msg.header.frame_id = PLANNING_FRAME_ID
|
||||
self.joint_pub.publish(joint_msg)
|
||||
publish_joint = False
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
def quit_handler(sig, frame):
|
||||
input_reader.shutdown()
|
||||
rclpy.shutdown()
|
||||
sys.exit(0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
signal.signal(signal.SIGINT, quit_handler)
|
||||
rclpy.init(args=sys.argv)
|
||||
input_reader = KeyboardReader()
|
||||
servo_keyboard = KeyboardServo()
|
||||
threading.Thread(target=servo_keyboard.spin).start()
|
||||
servo_keyboard.key_loop()
|
||||
@ -24,7 +24,9 @@ setup(
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'ps5_servo = ur_robotiq_servo.ps5_control:main'
|
||||
'ps5_servo = ur_robotiq_servo.ps5_control:main',
|
||||
'kb_servo = ur_robotiq_servo.kb_control:main',
|
||||
# 'servo_kb_input = ur_robotiq_servo.servo_keyboard_input:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
142
src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py
Normal file
142
src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py
Normal file
@ -0,0 +1,142 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Joy, JointState
|
||||
from geometry_msgs.msg import TwistStamped
|
||||
from control_msgs.msg import JointJog
|
||||
from std_srvs.srv import Trigger
|
||||
from std_msgs.msg import Float64MultiArray, MultiArrayDimension, MultiArrayLayout
|
||||
import keyboard
|
||||
|
||||
class KBControllerNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('kb_controller_node')
|
||||
# States
|
||||
self.mode = 'twist' # Initialize mode to 'twist'. Alternatives: 'twist', 'joint'
|
||||
self.last_button_state = 0 # Track the last state of the toggle button to detect presses
|
||||
self.last_finger_joint_angle = 0.0
|
||||
|
||||
# Parameters
|
||||
self.linear_speed_multiplier = self.declare_parameter('linear_speed_multiplier', 1.0)
|
||||
self.linear_speed_multiplier = self.get_parameter('linear_speed_multiplier').get_parameter_value().double_value
|
||||
self.get_logger().info(f"Linear speed multiplier: {self.linear_speed_multiplier}")
|
||||
|
||||
self.use_fake_hardware = self.declare_parameter('use_fake_hardware', False)
|
||||
self.use_fake_hardware = self.get_parameter('use_fake_hardware').get_parameter_value().bool_value
|
||||
self.get_logger().info(f"Use fake hardware: {self.use_fake_hardware}")
|
||||
|
||||
self.gripper_speed_multiplier = self.declare_parameter('gripper_speed_multiplier', 1.0)
|
||||
self.gripper_speed_multiplier = (self.get_parameter('gripper_speed_multiplier')
|
||||
.get_parameter_value().double_value)
|
||||
self.get_logger().info(f"Gripper speed multiplier: {self.gripper_speed_multiplier}")
|
||||
|
||||
self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 1.0)
|
||||
self.gripper_lower_limit = (self.get_parameter('gripper_lower_limit')
|
||||
.get_parameter_value().double_value)
|
||||
self.get_logger().info(f"Gripper lower limit: {self.gripper_lower_limit}")
|
||||
|
||||
self.gripper_upper_limit = self.declare_parameter('gripper_upper_limit', 1.0)
|
||||
self.gripper_upper_limit = (self.get_parameter('gripper_upper_limit')
|
||||
.get_parameter_value().double_value)
|
||||
self.get_logger().info(f"Gripper upper limit: {self.gripper_upper_limit}")
|
||||
# Subscriber and Publisher
|
||||
self.joint_state_sub = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_state_callback,
|
||||
10)
|
||||
|
||||
self.joy_sub = self.create_subscription(
|
||||
Joy,
|
||||
'/joy',
|
||||
self.keyboard_callback,
|
||||
10)
|
||||
|
||||
self.twist_pub = self.create_publisher(
|
||||
TwistStamped,
|
||||
'/servo_node/delta_twist_cmds',
|
||||
10)
|
||||
|
||||
self.joint_pub = self.create_publisher(
|
||||
JointJog,
|
||||
'/servo_node/delta_joint_cmds',
|
||||
10)
|
||||
|
||||
self.gripper_cmd_pub = self.create_publisher(
|
||||
Float64MultiArray,
|
||||
'/forward_gripper_position_controller/commands',
|
||||
10)
|
||||
|
||||
# Services
|
||||
self.servo_client = self.create_client(Trigger, '/servo_node/start_servo')
|
||||
|
||||
srv_msg = Trigger.Request()
|
||||
while not self.servo_client.wait_for_service(timeout_sec=1.0):
|
||||
self.get_logger().info('/servo_node/start_servo service not available, waiting again...')
|
||||
|
||||
self.call_start_servo()
|
||||
|
||||
self.get_logger().info('kb_servo_node started!')
|
||||
|
||||
def call_start_servo(self):
|
||||
request = Trigger.Request()
|
||||
future = self.servo_client.call_async(request)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
response = future.result()
|
||||
if response.success:
|
||||
self.get_logger().info(f'Successfully called start_servo: {response.message}')
|
||||
else:
|
||||
self.get_logger().info('Failed to call start_servo')
|
||||
|
||||
def joint_state_callback(self, msg):
|
||||
try:
|
||||
index = msg.name.index('finger_joint')
|
||||
self.last_finger_joint_angle = msg.position[index]
|
||||
except ValueError:
|
||||
self.get_logger().error('finger_joint not found in /joint_states msg')
|
||||
|
||||
def keyboard_callback(self, event):
|
||||
# Process keyboard events
|
||||
print("Key pressed")
|
||||
if event.event_type == keyboard.KEY_DOWN:
|
||||
# Handle key presses
|
||||
if event.name == 't':
|
||||
# Toggle mode between twist and joint control
|
||||
self.mode = 'joint' if self.mode == 'twist' else 'twist'
|
||||
self.get_logger().info(f'Mode switched to: {self.mode}')
|
||||
elif event.name == 'w':
|
||||
# Move forward
|
||||
self.publish_twist(1.0 * self.linear_speed_multiplier) # Adjust speed as needed
|
||||
self.get_logger().info('Moving forward')
|
||||
elif event.name == 's':
|
||||
# Move backward
|
||||
self.publish_twist(-1.0 * self.linear_speed_multiplier) # Adjust speed as needed
|
||||
self.get_logger().info('Moving backward')
|
||||
elif event.event_type == keyboard.KEY_UP:
|
||||
# Handle key releases
|
||||
if event.name == 'w' or event.name == 's':
|
||||
# Stop moving
|
||||
self.publish_twist(0.0)
|
||||
self.get_logger().info('Stopped moving')
|
||||
|
||||
def publish_twist(self, linear_speed):
|
||||
twist_msg = TwistStamped()
|
||||
twist_msg.header.frame_id = 'tool0'
|
||||
twist_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
twist_msg.twist.linear.x = linear_speed
|
||||
twist_msg.twist.linear.y = 0.0 # Adjust as needed
|
||||
twist_msg.twist.linear.z = 0.0 # Adjust as needed
|
||||
twist_msg.twist.angular.x = 0.0
|
||||
twist_msg.twist.angular.y = 0.0
|
||||
twist_msg.twist.angular.z = 0.0
|
||||
self.twist_pub.publish(twist_msg)
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = KBControllerNode()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Loading…
Reference in New Issue
Block a user