qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added homing service to franka hand node

Browse Source
This commit is contained in:
qlin960618
2024-12-10 23:44:07 +09:00
parent 4f0c5fb97d
commit 787228d507
3 changed files with 49 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(sas_common REQUIRED)
find_package(sas_core REQUIRED)
find_package(sas_msgs REQUIRED)
@@ -156,7 +157,7 @@ target_link_libraries(sas_robot_driver_franka
add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp)
ament_target_dependencies(qros_effector_driver_franka_hand
rclcpp sas_common sas_core
rclcpp sas_common sas_core std_srvs
sas_robot_driver_franka_interfaces
)

12
include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp
View File

@@ -56,7 +56,7 @@
#include <sas_robot_driver_franka_interfaces/srv/move.hpp>
#include <sas_robot_driver_franka_interfaces/msg/gripper_state.hpp>
#endif
#include <std_srvs/srv/trigger.hpp>
// using namespace DQ_robotics;
// using namespace Eigen;
@@ -99,19 +99,25 @@ private:
std::mutex gripper_in_use_;
Service<srv::Grasp>::SharedPtr grasp_srv_;
Service<srv::Move>::SharedPtr move_srv_;
Service<std_srvs::srv::Trigger>::SharedPtr homing_srv_;
public:
bool _grasp_srv_callback(
const std::shared_ptr<srv::Grasp::Request> req,
const std::shared_ptr<srv::Grasp::Request> &req,
std::shared_ptr<srv::Grasp::Response> res
);
bool _move_srv_callback(
const std::shared_ptr<srv::Move::Request> req,
const std::shared_ptr<srv::Move::Request> &req,
std::shared_ptr<srv::Move::Response> res
);
bool _home_srv_callback(
const std::shared_ptr<std_srvs::srv::Trigger::Request> &req,
std::shared_ptr<std_srvs::srv::Trigger::Response> res
);
EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete;
EffectorDriverFrankaHand()=delete;
~EffectorDriverFrankaHand();

44
src/hand/qros_effector_driver_franka_hand.cpp
View File

@@ -63,6 +63,8 @@ namespace qros
std::bind(&EffectorDriverFrankaHand::_grasp_srv_callback, this, _1, _2));
move_srv_ = node->create_service<srv::Move>(driver_node_prefix_ + "/move",
std::bind(&EffectorDriverFrankaHand::_move_srv_callback, this, _1, _2));
homing_srv_ = node->create_service<std_srvs::srv::Trigger>(driver_node_prefix_ + "/homing",
std::bind(&EffectorDriverFrankaHand::_home_srv_callback, this, _1, _2));
gripper_status_publisher_ = node->create_publisher<msg::GripperState>(
driver_node_prefix_ + "/gripper_status", 1);
}
@@ -177,7 +179,7 @@ namespace qros
}
bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr<srv::Grasp::Request> req, std::shared_ptr<srv::Grasp::Response> res)
bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr<srv::Grasp::Request> &req, std::shared_ptr<srv::Grasp::Response> res)
{
RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
auto force = req->force;
@@ -192,13 +194,13 @@ namespace qros
RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
bool ret = false;
bool function_ret = true;
gripper_in_use_.lock();
#ifdef IN_TESTING
ret = true;
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
#else
try
{
std::lock_guard<std::mutex> lock(gripper_in_use_);
ret = gripper_sptr_->grasp(req->width, speed, force, epsilon_inner, epsilon_outer);
}catch(franka::CommandException& e)
{
@@ -210,13 +212,12 @@ namespace qros
function_ret = false;
}
#endif
gripper_in_use_.unlock();
res->set__success(ret);
return function_ret;
}
bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr<srv::Move::Request> req, std::shared_ptr<srv::Move::Response> res)
bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr<srv::Move::Request> &req, std::shared_ptr<srv::Move::Response> res)
{
RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
auto speed = req->speed;
@@ -224,13 +225,13 @@ namespace qros
RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req->width));
bool ret = false;
bool function_ret = true;
gripper_in_use_.lock();
#ifdef IN_TESTING
ret = true;
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
#else
try
{
std::lock_guard<std::mutex> lock(gripper_in_use_);
ret = gripper_sptr_->move(req->width, speed);
}catch(franka::CommandException& e)
{
@@ -242,7 +243,38 @@ namespace qros
function_ret = false;
}
#endif
gripper_in_use_.unlock();
res->set__success(ret);
return function_ret;
}
bool EffectorDriverFrankaHand::_home_srv_callback(const std::shared_ptr<std_srvs::srv::Trigger::Request> &req, std::shared_ptr<std_srvs::srv::Trigger::Response> res)
{
RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::Homing...");
bool ret = false;
bool function_ret = true;
#ifdef IN_TESTING
ret = true;
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
#else
try
{
std::lock_guard<std::mutex> lock(gripper_in_use_);
ret = gripper_sptr_->homing();
}catch(franka::CommandException& e)
{
RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::CommandException::" + std::string(e.what()));
res->set__message("_home_srv_callback::CommandException::" + std::string(e.what()));
function_ret = false;
ret = false;
}catch(franka::NetworkException& e)
{
RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::NetworkException::" + std::string(e.what()));
res->set__message("_home_srv_callback::NetworkException::" + std::string(e.what()));
function_ret = false;
ret = false;
}
#endif
res->set__success(ret);
return function_ret;
}