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

migration to ros2 done

Browse Source
This commit is contained in:
qlin960618
2024-07-27 13:07:37 +09:00
parent f5552be8d6
commit 7dfd4d40b8
94 changed files with 13216 additions and 2224 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
src/sas_robot_driver_franka_node.cpp
View File

@@ -26,6 +26,8 @@
# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
# - Adapted from sas_robot_driver_denso_node.cpp
# (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp)
# 2. Quentin Lin
# - Adaption to ROS2
#
# ################################################################
*/
@@ -36,11 +38,11 @@
#include <exception>
#include <dqrobotics/utils/DQ_Math.h>
#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
#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.h"
#include <robot_dynamic/qros_robot_dynamics_provider.h>
#include <sas_common/sas_common.hpp>
#include <sas_conversions/eigen3_std_conversions.hpp>
#include <sas_robot_driver/sas_robot_driver_ros.hpp>
#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
/*********************************************
@@ -80,67 +82,68 @@ int main(int argc, char **argv)
{
if(signal(SIGINT, sig_int_handler) == SIG_ERR)
{
throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
throw std::runtime_error("Error setting the signal int handler.");
}
ros::init(argc, argv, "sas_robot_driver_franka_node", ros::init_options::NoSigintHandler);
ROS_WARN("=====================================================================");
ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------");
ROS_WARN("=====================================================================");
ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_node");
ros::NodeHandle nh;
const auto node_name = std::string(node->get_name());
RCLCPP_WARN(node->get_logger(),"=====================================================================");
RCLCPP_WARN(node->get_logger(),"-----------------Adapted by Quentin Lin ------------------------");
RCLCPP_WARN(node->get_logger(),"=====================================================================");
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server.");
sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
RobotInterfaceFranka::FrankaInterfaceConfiguration franka_interface_configuration;
sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address);
sas::get_ros_param(nh,"/robot_mode", robot_driver_franka_configuration.mode);
sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_configuration.ip_address);
sas::get_ros_parameter(node,"robot_mode", robot_driver_franka_configuration.mode);
double upper_scale_factor = 1.0;
std::vector<std::string> all_params;
if(nh.hasParam(ros::this_node::getName()+"/force_upper_limits_scaling_factor"))
if(node->has_parameter("force_upper_limits_scaling_factor"))
{
sas::get_ros_param(nh,"/force_upper_limits_scaling_factor",upper_scale_factor);
ROS_WARN_STREAM(ros::this_node::getName()+"::Set force upper limits scaling factor: " << upper_scale_factor);
sas::get_ros_parameter(node,"force_upper_limits_scaling_factor",upper_scale_factor);
RCLCPP_WARN_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Set force upper limits scaling factor: " << upper_scale_factor);
}
if(nh.hasParam(ros::this_node::getName()+"/upper_torque_threshold")) {
ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold override and set.");
if(node->has_parameter("upper_torque_threshold")) {
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
std::vector<double> upper_torque_threshold_std_vec;
sas::get_ros_param(nh,"/upper_torque_threshold",upper_torque_threshold_std_vec);
sas::get_ros_parameter(node,"upper_torque_threshold",upper_torque_threshold_std_vec);
franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
}else {
ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold not set. Using default with value scalling.");
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);
}
if(nh.hasParam(ros::this_node::getName()+"/upper_force_threshold")) {
ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold override and set.");
if(node->has_parameter("upper_force_threshold")) {
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_param(nh,"/upper_force_threshold",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 {
ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold not set. Using default with value scalling.");
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 = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
}
if(nh.hasParam(ros::this_node::getName()+"/robot_parameter_file_path"))
if(node->has_parameter("robot_parameter_file_path"))
{
std::string robot_parameter_file_path;
sas::get_ros_param(nh,"/robot_parameter_file_path",robot_parameter_file_path);
ROS_INFO_STREAM(ros::this_node::getName()+"::Loading robot parameters from file: " + 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{ROS_INFO_STREAM(ros::this_node::getName()+"::Robot parameter file path not set. Robot Model Unknow.");}
}else{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;
sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
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);
sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_ros_configuration.thread_sampling_time_sec);
sas::get_ros_parameter(node,"q_min",robot_driver_ros_configuration.q_min);
sas::get_ros_parameter(node,"q_max",robot_driver_ros_configuration.q_max);
// initialize the robot dynamic provider
robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
qros::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
robot_driver_ros_configuration.robot_driver_provider_prefix = node_name;
qros::RobotDynamicsServer robot_dynamic_provider(node, robot_driver_ros_configuration.robot_driver_provider_prefix);
if(robot_driver_franka_configuration.robot_reference_frame!=0)
{
robot_dynamic_provider.set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame);
@@ -148,16 +151,17 @@ int main(int argc, char **argv)
try
{
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka robot.");
sas::RobotDriverFranka robot_driver_franka(
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating Franka robot.");
auto robot_driver_franka = std::make_shared<sas::RobotDriverFranka>(
node,
&robot_dynamic_provider,
robot_driver_franka_configuration,
&kill_this_process
);
//robot_driver_franka.set_joint_limits({qmin, qmax});
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
sas::RobotDriverROS robot_driver_ros(nh,
&robot_driver_franka,
RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating RobotDriverROS.");
sas::RobotDriverROS robot_driver_ros(node,
robot_driver_franka,
robot_driver_ros_configuration,
&kill_this_process);
robot_driver_ros.control_loop();
@@ -165,7 +169,7 @@ int main(int argc, char **argv)
catch (const std::exception& e)
{
kill_this_process = true;
ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
RCLCPP_ERROR_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Exception::" + e.what());
}