91 lines
3.6 KiB
C++
91 lines
3.6 KiB
C++
//#include "ros/ros.h"
|
|
//#include "std_msgs/String.h"
|
|
|
|
#include <sstream>
|
|
|
|
#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"
|
|
|
|
|
|
/*********************************************
|
|
* SIGNAL HANDLER
|
|
* *******************************************/
|
|
#include<signal.h>
|
|
static std::atomic_bool kill_this_process(false);
|
|
void sig_int_handler(int)
|
|
{
|
|
kill_this_process = true;
|
|
}
|
|
|
|
|
|
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.");
|
|
}
|
|
|
|
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.");
|
|
|
|
|
|
ros::NodeHandle nh;
|
|
sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
|
|
|
|
sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address);
|
|
// sas::get_ros_param(nh,"/robot_port", robot_driver_franka_configuration.port);
|
|
//sas::get_ros_param(nh,"/robot_speed", robot_driver_franka_configuration.speed);
|
|
|
|
//robot_driver_franka_configuration.ip_address = "172.16.0.2";
|
|
//robot_driver_franka_configuration.port = 0;
|
|
//robot_driver_franka_configuration.speed = 0.0;
|
|
|
|
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);
|
|
|
|
//auto qmin = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_min);
|
|
//auto qmax = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_max);
|
|
|
|
|
|
//std::vector<double> q_min = {-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895};
|
|
//std::vector<double> q_max = { 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895};
|
|
|
|
//robot_driver_ros_configuration.thread_sampling_time_nsec = 4000000;
|
|
//robot_driver_ros_configuration.q_min = q_min;
|
|
//robot_driver_ros_configuration.q_max = q_max;
|
|
|
|
robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
|
|
|
|
try
|
|
{
|
|
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka robot.");
|
|
sas::RobotDriverFranka robot_driver_franka(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,
|
|
robot_driver_ros_configuration,
|
|
&kill_this_process);
|
|
robot_driver_ros.control_loop();
|
|
}
|
|
catch (const std::exception& e)
|
|
{
|
|
ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
|
|
}
|
|
|
|
|
|
return 0;
|
|
}
|