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

added gripper driver for service control

Browse Source
This commit is contained in:
qlin1806
2024-07-22 05:25:28 -07:00
parent 25e88f56ed
commit 101c45c228
19 changed files with 1805 additions and 305 deletions
Download Patch File Download Diff File Expand all files Collapse all files

292
src/hand/qros_effector_driver_franka_hand.cpp Normal file
View File

@@ -0,0 +1,292 @@
/*
# Copyright (c) 2023 Juan Jose Quiroz Omana
#
# This file is part of sas_robot_driver_franka.
#
# sas_robot_driver_franka is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# sas_robot_driver_franka is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with sas_robot_driver. If not, see <https://www.gnu.org/licenses/>.
#
# ################################################################
#
# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
#
# ################################################################
#
# Contributors:
# 1. Quenitin Lin
#
# ################################################################
*/
#include "hand/qros_effector_driver_franka_hand.h"
#include <franka/exception.h>
namespace qros {
//const static int SLAVE_MODE_JOINT_CONTROL;
//const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
EffectorDriverFrankaHand::~EffectorDriverFrankaHand()
{
if(_is_connected())
{
disconnect();
}
}
EffectorDriverFrankaHand::EffectorDriverFrankaHand(
const std::string &driver_node_prefix,
const EffectorDriverFrankaHandConfiguration& configuration,
ros::NodeHandle& node_handel,
std::atomic_bool* break_loops):
driver_node_prefix_(driver_node_prefix),
configuration_(configuration),
node_handel_(node_handel),
break_loops_(break_loops)
{
gripper_sptr_ = nullptr;
grasp_server_ = node_handel_.advertiseService(driver_node_prefix_+"/grasp", &EffectorDriverFrankaHand::_grasp_srv_callback, this);
move_server_ = node_handel_.advertiseService(driver_node_prefix_+"/move", &EffectorDriverFrankaHand::_move_srv_callback, this);
gripper_status_publisher_ = node_handel_.advertise<sas_robot_driver_franka::GripperState>(driver_node_prefix_+"/gripper_status", 1);
}
bool EffectorDriverFrankaHand::_is_connected() const
{
#ifdef IN_TESTING
return true;
#endif
if(gripper_sptr_ == nullptr) return false;
if(!gripper_sptr_) return false;
else return true;
}
void EffectorDriverFrankaHand::start_control_loop()
{
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns);
clock.init();
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
while(!(*break_loops_))
{
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
if(!status_loop_running_){
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
break_loops_->store(true);
break;
}
clock.update_and_sleep();
ros::spinOnce();
}
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
}
void EffectorDriverFrankaHand::connect()
{
#ifdef IN_TESTING
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
return;
#endif
gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::Could not connect to the robot.");
}
void EffectorDriverFrankaHand::disconnect() noexcept
{
#ifdef IN_TESTING
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
return;
#endif
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnecting...");
gripper_sptr_->~Gripper();
gripper_sptr_ = nullptr;
}
/**
* @brief robot_driver_franka::gripper_homing
*/
void EffectorDriverFrankaHand::gripper_homing()
{
#ifdef IN_TESTING
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
return;
#endif
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected.");
auto ret = gripper_sptr_->homing();
if(!ret)
{
throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
}
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
}
void EffectorDriverFrankaHand::initialize()
{
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
gripper_homing();
// start gripper status loop
status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
}
void EffectorDriverFrankaHand::deinitialize()
{
#ifdef IN_TESTING
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
return;
#endif
if(_is_connected())
{
franka::GripperState gripper_state = gripper_sptr_->readOnce();
if(gripper_state.is_grasped)
{
gripper_sptr_->stop();
}
}
}
bool EffectorDriverFrankaHand::_grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req, sas_robot_driver_franka::Grasp::Response& res)
{
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req.width));
auto force = req.force;
auto speed = req.speed;
auto epsilon_inner = req.epsilon_inner;
auto epsilon_outer = req.epsilon_outer;
if(force <= 0.0) force = configuration_.default_force;
if(speed<= 0.0) speed = configuration_.default_speed;
if(epsilon_inner <= 0.0) epsilon_inner = configuration_.default_epsilon_inner;
if(epsilon_outer <= 0.0) epsilon_outer = configuration_.default_epsilon_outer;
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[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;
#else
try
{
ret = gripper_sptr_->grasp(req.width, speed, force, epsilon_inner, epsilon_outer);
}catch(franka::CommandException& e)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + e.what());
function_ret = false;
}catch(franka::NetworkException& e)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + e.what());
function_ret = false;
}
#endif
gripper_in_use_.unlock();
res.success = ret;
return function_ret;
}
bool EffectorDriverFrankaHand::_move_srv_callback(sas_robot_driver_franka::Move::Request& req, sas_robot_driver_franka::Move::Response& res)
{
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
auto speed = req.speed;
if (speed <= 0.0) speed = configuration_.default_speed;
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[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;
#else
try
{
ret = gripper_sptr_->move(req.width, speed);
}catch(franka::CommandException& e)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + e.what());
function_ret = false;
}catch(franka::NetworkException& e)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + e.what());
function_ret = false;
}
#endif
gripper_in_use_.unlock();
res.success = ret;
return function_ret;
}
void EffectorDriverFrankaHand::_gripper_status_loop()
{
status_loop_running_ = true;
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns);
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.");
clock.init();
try
{
while (status_loop_running_)
{
bool should_unlock = false;
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
try
{
#ifdef IN_TESTING
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
#endif
gripper_in_use_.lock();
should_unlock = true;
franka::GripperState gripper_state = gripper_sptr_->readOnce();
gripper_in_use_.unlock();
sas_robot_driver_franka::GripperState msg;
msg.width = static_cast<float>(gripper_state.width);
msg.max_width = static_cast<float>(gripper_state.max_width);
msg.is_grasped = gripper_state.is_grasped;
msg.temperature = gripper_state.temperature;
msg.duration_ms = gripper_state.time.toMSec();
gripper_status_publisher_.publish(msg);
}catch(...)
{
if (should_unlock) gripper_in_use_.unlock();
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
sas_robot_driver_franka::GripperState msg;
msg.width = 0.0;
gripper_status_publisher_.publish(msg);
}
clock.update_and_sleep();
}
status_loop_running_=false;
}catch (std::exception& e)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.what());
status_loop_running_=false;
}
catch (...)
{
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
status_loop_running_=false;
}
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
}
} // qros

125
src/hand/qros_effector_driver_franka_hand.h Normal file
View File

@@ -0,0 +1,125 @@
#pragma once
/*
# Copyright (c) 2023 Juan Jose Quiroz Omana
#
# This file is part of sas_robot_driver_franka.
#
# sas_robot_driver_franka is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# sas_robot_driver_franka is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with sas_robot_driver. If not, see <https://www.gnu.org/licenses/>.
#
# ################################################################
#
# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
#
# ################################################################
#
# Contributors:
# 1. Quenitin Lin
#
# ################################################################
*/
#include <exception>
#include <tuple>
#include <atomic>
#include <vector>
#include <memory>
#include <franka/robot.h>
#include <franka/gripper.h>
#include <thread>
#include <mutex>
// #define IN_TESTING
// #include <sas_robot_driver/sas_robot_driver.h>
#include <sas_clock/sas_clock.h>
#include <ros/ros.h>
#include <ros/service.h>
#include <sas_robot_driver_franka/Grasp.h>
#include <sas_robot_driver_franka/Move.h>
#include <sas_robot_driver_franka/GripperState.h>
#ifdef IN_TESTING
#include <Move.h> // dummy include
#include <Grasp.h> // dummy include
#include <GripperState.h> // dummy include
#endif
// using namespace DQ_robotics;
// using namespace Eigen;
namespace qros {
struct EffectorDriverFrankaHandConfiguration
{
std::string robot_ip;
int thread_sampeling_time_ns = 1e8; // 10Hz
double default_force = 3.0;
double default_speed = 0.1;
double default_epsilon_inner = 0.005;
double default_epsilon_outer = 0.005;
};
class EffectorDriverFrankaHand{
private:
std::string driver_node_prefix_;
EffectorDriverFrankaHandConfiguration configuration_;
ros::NodeHandle& node_handel_;
std::shared_ptr<franka::Gripper> gripper_sptr_;
std::atomic_bool* break_loops_;
bool _is_connected() const;
// thread specific functions
void _gripper_status_loop();
std::thread status_loop_thread_;
std::atomic_bool status_loop_running_{false};
ros::Publisher gripper_status_publisher_;
std::mutex gripper_in_use_;
ros::ServiceServer grasp_server_;
ros::ServiceServer move_server_;
public:
bool _grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req, sas_robot_driver_franka::Grasp::Response& res);
bool _move_srv_callback(sas_robot_driver_franka::Move::Request& req, sas_robot_driver_franka::Move::Response& res);
EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete;
EffectorDriverFrankaHand()=delete;
~EffectorDriverFrankaHand();
EffectorDriverFrankaHand(
const std::string &driver_node_prefix,
const EffectorDriverFrankaHandConfiguration& configuration,
ros::NodeHandle& node_handel,
std::atomic_bool* break_loops);
void start_control_loop();
void gripper_homing();
void connect() ;
void disconnect() noexcept;
void initialize() ;
void deinitialize() ;
};
} // qros

152
src/hand/sas_robot_driver_franka_hand.cpp
View File

@@ -1,152 +0,0 @@
/*
# Copyright (c) 2023 Juan Jose Quiroz Omana
#
# This file is part of sas_robot_driver_franka.
#
# sas_robot_driver_franka is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# sas_robot_driver_franka is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with sas_robot_driver. If not, see <https://www.gnu.org/licenses/>.
#
# ################################################################
#
# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
#
# ################################################################
#
# Contributors:
# 1. Quenitin Lin
#
# ################################################################
*/
#include "sas_robot_driver_franka_hand.h"
namespace sas {
//const static int SLAVE_MODE_JOINT_CONTROL;
//const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
RobotDriverFrankaHand::~RobotDriverFrankaHand()
{
if(_is_connected())
{
disconnect();
}
}
RobotDriverFrankaHand::RobotDriverFrankaHand(
const RobotDriverFrankaHandConfiguration& configuration,
const RobotDriverROSConfiguration& ros_configuration,
std::atomic_bool* break_loops):
configuration_(configuration), ros_configuration_(ros_configuration), break_loops_(break_loops)
{
gripper_sptr_ = nullptr;
}
bool RobotDriverFrankaHand::_is_connected() const
{
if(gripper_sptr_ == nullptr) return false;
if(!gripper_sptr_) return false;
else return true;
}
VectorXd RobotDriverFrankaHand::get_joint_positions()
{
return joint_positions_;
}
void RobotDriverFrankaHand::set_target_joint_positions(const VectorXd& desired_joint_positions_rad)
{
}
VectorXd RobotDriverFrankaHand::get_joint_velocities()
{
return VectorXd::Zero(1);
}
void RobotDriverFrankaHand::set_target_joint_velocities(const VectorXd& desired_joint_velocities)
{
}
VectorXd RobotDriverFrankaHand::get_joint_forces()
{
return VectorXd::Zero(1);
}
void RobotDriverFrankaHand::start_control_loop()
{
Clock clock = Clock(ros_configuration_.thread_sampling_time_nsec);
clock.init();
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Starting control loop.");
while(!(*break_loops_))
{
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Robot is not connected.");
clock.update_and_sleep();
}
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Exiting control loop.");
}
void RobotDriverFrankaHand::connect()
{
gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::connect::Could not connect to the robot.");
}
void RobotDriverFrankaHand::disconnect() noexcept
{
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::disconnecting...");
gripper_sptr_->~Gripper();
gripper_sptr_ = nullptr;
}
/**
* @brief robot_driver_franka::gripper_homing
*/
void RobotDriverFrankaHand::gripper_homing()
{
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Robot is not connected.");
auto ret = gripper_sptr_->homing();
if(!ret)
{
throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
}
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Gripper homed.");
}
void RobotDriverFrankaHand::initialize()
{
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::initialize::Robot is not connected.");
gripper_homing();
}
void RobotDriverFrankaHand::deinitialize()
{
if(_is_connected())
{
franka::GripperState gripper_state = gripper_sptr_->readOnce();
if(gripper_state.is_grasped)
{
gripper_sptr_->stop();
}
}
}
} // sas

117
src/hand/sas_robot_driver_franka_hand.h
View File

@@ -1,117 +0,0 @@
#pragma once
/*
# Copyright (c) 2023 Juan Jose Quiroz Omana
#
# This file is part of sas_robot_driver_franka.
#
# sas_robot_driver_franka is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# sas_robot_driver_franka is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with sas_robot_driver. If not, see <https://www.gnu.org/licenses/>.
#
# ################################################################
#
# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
#
# ################################################################
#
# Contributors:
# 1. Quenitin Lin
#
# ################################################################
*/
#include <exception>
#include <tuple>
#include <atomic>
#include <vector>
#include <memory>
#include <franka/robot.h>
#include <franka/gripper.h>
// #include <thread>
#include <dqrobotics/DQ.h>
#include <sas_robot_driver/sas_robot_driver.h>
#include <sas_robot_driver/sas_robot_driver_ros.h>
#include <sas_clock/sas_clock.h>
#include <ros/common.h>
using namespace DQ_robotics;
using namespace Eigen;
namespace sas {
struct RobotDriverFrankaHandConfiguration
{
std::string robot_ip;
};
class RobotDriverFrankaHand{
private:
RobotDriverFrankaHandConfiguration configuration_;
RobotDriverROSConfiguration ros_configuration_;
std::shared_ptr<franka::Gripper> gripper_sptr_;
//Joint positions
VectorXd joint_positions_;
//VectorXd joint_velocities_;
//VectorXd end_effector_pose_;
// std::thread control_loop_thread_;
std::atomic_bool* break_loops_;
bool _is_connected() const;
public:
//const static int SLAVE_MODE_JOINT_CONTROL;
//const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
RobotDriverFrankaHand(const RobotDriverFrankaHand&)=delete;
RobotDriverFrankaHand()=delete;
~RobotDriverFrankaHand();
RobotDriverFrankaHand(
const RobotDriverFrankaHandConfiguration& configuration,
const RobotDriverROSConfiguration& ros_configuration,
std::atomic_bool* break_loops);
void start_control_loop();
VectorXd get_joint_positions();
void set_target_joint_positions(const VectorXd& desired_joint_positions_rad);
VectorXd get_joint_velocities();
void set_target_joint_velocities(const VectorXd& desired_joint_velocities);
VectorXd get_joint_forces();
void gripper_homing();
void connect() ;
void disconnect() noexcept;
void initialize() ;
void deinitialize() ;
//bool set_end_effector_pose_dq(const DQ& pose);
//DQ get_end_effector_pose_dq();
};
} // sas

46
src/sas_robot_driver_franka_hand_node.cpp
View File

@@ -36,7 +36,7 @@
#include <sas_common/sas_common.h>
#include <sas_conversions/eigen3_std_conversions.h>
#include <sas_robot_driver/sas_robot_driver_ros.h>
#include "sas_robot_driver_franka_hand.h"
#include "hand/qros_effector_driver_franka_hand.h"
/*********************************************
@@ -49,6 +49,22 @@ void sig_int_handler(int)
kill_this_process = true;
}
template<typename T>
void get_optional_parameter(const std::string &node_prefix, ros::NodeHandle &nh, const std::string &param_name, T &param)
{
if(nh.hasParam(node_prefix + param_name))
{
sas::get_ros_param(nh,param_name,param);
}else
{
ROS_INFO_STREAM(ros::this_node::getName() + "::Parameter " + param_name + " not found. Using default value. " + std::to_string(param));
}
}
int main(int argc, char **argv)
{
if(signal(SIGINT, sig_int_handler) == SIG_ERR)
@@ -60,32 +76,24 @@ int main(int argc, char **argv)
ROS_WARN("---------------------------Quentin Lin-------------------------------");
ROS_WARN("=====================================================================");
ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
const std::string& effector_driver_provider_prefix = ros::this_node::getName();
ros::NodeHandle nh;
sas::RobotDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
get_optional_parameter(effector_driver_provider_prefix,nh,"/thread_sampling_time_nsec",robot_driver_franka_hand_configuration.thread_sampeling_time_ns);
get_optional_parameter(effector_driver_provider_prefix,nh,"/default_force",robot_driver_franka_hand_configuration.default_force);
get_optional_parameter(effector_driver_provider_prefix,nh,"/default_speed",robot_driver_franka_hand_configuration.default_speed);
get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_inner",robot_driver_franka_hand_configuration.default_epsilon_inner);
get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_outer",robot_driver_franka_hand_configuration.default_epsilon_outer);
sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
bool q_lim_override = false;
if(nh.hasParam("q_min") || nh.hasParam("q_max"))
{
sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
q_lim_override = true;
}else
{
}
robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
sas::RobotDriverFrankaHand franka_hand_driver(
qros::EffectorDriverFrankaHand franka_hand_driver(
effector_driver_provider_prefix,
robot_driver_franka_hand_configuration,
robot_driver_ros_configuration,
nh,
&kill_this_process
);
try