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

attempt to merge newer changes

Browse Source
This commit is contained in:
qlin1806
2024-08-21 16:35:22 -07:00
parent 0358b851df fe5bb31873
commit 35131ed59e
5 changed files with 87 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/hand/qros_effector_driver_franka_hand.cpp
View File

@@ -78,12 +78,11 @@ namespace qros
}
void EffectorDriverFrankaHand::start_control_loop()
{
void EffectorDriverFrankaHand::start_control_loop() {
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
clock.init();
RCLCPP_INFO_STREAM(node_->get_logger(),
"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
RCLCPP_WARN_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::Gripper READY.");
while (!(*break_loops_))
{
if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
@@ -141,7 +140,7 @@ namespace qros
{
throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
}
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
}
void EffectorDriverFrankaHand::initialize()
@@ -151,6 +150,14 @@ namespace qros
gripper_homing();
// start gripper status loop
status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
// check status loop with timeout
auto time_now = std::chrono::system_clock::now();
auto time_out = time_now + std::chrono::seconds(5);
while (!status_loop_running_)
{
if (std::chrono::system_clock::now() > time_out){throw std::runtime_error("[" + std::string(node_->get_name()) + "]::[EffectorDriverFrankaHand]::initialize::Could not start status loop.");}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
void EffectorDriverFrankaHand::deinitialize()
@@ -243,13 +250,12 @@ namespace qros
void EffectorDriverFrankaHand::_gripper_status_loop()
{
status_loop_running_ = true;
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.")
;
RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.");
clock.init();
try
{
status_loop_running_ = true;
while (status_loop_running_)
{
#ifdef BLOCK_READ_IN_USED

2
src/robot_dynamics/qros_robot_dynamics_py.cpp
View File

@@ -40,7 +40,7 @@ using RDC = qros::RobotDynamicsClient;
using RDS = qros::RobotDynamicsServer;
PYBIND11_MODULE(_qros_franka_robot_dynamic, m)
PYBIND11_MODULE(_qros_franka_robot_dynamics_py, m)
{
py::class_<RDC>(m, "RobotDynamicsClient")
.def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())

28
src/sas_robot_driver_franka_node.cpp
View File

@@ -118,35 +118,33 @@ int main(int argc, char **argv)
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"Force upper limits scaling factor is not set.");
}
try{node->declare_parameter<std::vector<double>>("upper_torque_threshold");}catch (...){}
if(node->has_parameter("upper_torque_threshold")) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
try {
std::vector<double> upper_torque_threshold_std_vec;
sas::get_ros_parameter(node,"upper_torque_threshold",upper_torque_threshold_std_vec);
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
}else {
}catch(...) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor);
}
try{node->declare_parameter<std::vector<double>>("upper_force_threshold");}catch (...){}
if(node->has_parameter("upper_force_threshold")) {
try {
std::vector<double> upper_force_threshold_std_vec;
sas::get_ros_parameter(node,"upper_force_threshold",upper_force_threshold_std_vec);
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold override and set.");
std::vector<double> upper_torque_threshold_std_vec;
sas::get_ros_parameter(node,"upper_force_threshold",upper_torque_threshold_std_vec);
franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_torque_threshold_std_vec);
}else {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold not set. Using default with value scalling.");
franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_force_threshold_std_vec);
}catch(...) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
}
try{node->declare_parameter<std::string>("robot_parameter_file_path");}catch (...){}
if(node->has_parameter("robot_parameter_file_path"))
{
try {
std::string robot_parameter_file_path;
sas::get_ros_parameter(node,"robot_parameter_file_path",robot_parameter_file_path);
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading robot parameters from file: " + robot_parameter_file_path);
const auto robot = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(robot_parameter_file_path);
robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame();
}else{RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");}
}catch(...) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");
}
robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;