From e45a160fd0d7cb943c91d59c6c142d57d4e2ecd4 Mon Sep 17 00:00:00 2001 From: Juancho Date: Wed, 2 Aug 2023 22:19:34 +0900 Subject: [PATCH] Added a new class --- CMakeLists.txt | 12 +- include/robot_coppelia_ros_interface.h | 68 ++++++++++ src/robot_coppelia_ros_interface.cpp | 145 ++++++++++++++++++++++ src/robot_coppelia_ros_interface_node.cpp | 115 +++++++++++++++++ 4 files changed, 339 insertions(+), 1 deletion(-) create mode 100644 include/robot_coppelia_ros_interface.h create mode 100644 src/robot_coppelia_ros_interface.cpp create mode 100644 src/robot_coppelia_ros_interface_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 31ad27f..cec858c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,7 +67,6 @@ target_link_libraries(CustomMotionGeneration dqrobotics ConstraintsManager) - add_library(robot_interface_franka src/robot_interface_franka.cpp) target_link_libraries(robot_interface_franka Franka::Franka dqrobotics @@ -113,6 +112,17 @@ target_link_libraries(sas_robot_driver_franka_node sas_robot_driver_franka ${catkin_LIBRARIES}) +add_library(robot_coppelia_ros_interface src/robot_coppelia_ros_interface.cpp) +target_link_libraries(robot_coppelia_ros_interface + dqrobotics + dqrobotics-interface-vrep + ${catkin_LIBRARIES}) + +add_executable(robot_coppelia_ros_interface_node src/robot_coppelia_ros_interface_node.cpp) +target_link_libraries(robot_coppelia_ros_interface_node + robot_coppelia_ros_interface + sas_robot_driver_franka + ${catkin_LIBRARIES}) add_executable(JuankaEmika diff --git a/include/robot_coppelia_ros_interface.h b/include/robot_coppelia_ros_interface.h new file mode 100644 index 0000000..4edbe9c --- /dev/null +++ b/include/robot_coppelia_ros_interface.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace Eigen; + + +struct RobotCoppeliaROSConfiguration +{ + + int thread_sampling_time_nsec; + int port; + std::string ip; + +}; + +class RobotCoppeliaRosInterface +{ +private: + std::atomic_bool* kill_this_node_; + bool _should_shutdown() const; + sas::Clock clock_; + RobotCoppeliaROSConfiguration configuration_; + +protected: + ros::NodeHandle nh_; + VectorXd joint_positions_; + VectorXd target_joint_positions_; + std::string topic_prefix_; + ros::Publisher publisher_target_joint_positions_; + ros::Publisher publisher_joint_states_; + //ros::Subscriber subscriber_joint_state_; + ros::Subscriber subscriber_target_joint_positions_; + ros::Subscriber subscriber_target_joint_velocities_; + std::shared_ptr coppelia_robot_; + std::vector jointnames_; + std::shared_ptr vi_; + void _joint_states_callback(const sensor_msgs::JointState::ConstPtr& jointstate); + void _callback_target_joint_positions(const std_msgs::Float64MultiArrayConstPtr &msg); + + void send_joint_states(const VectorXd &joint_positions, const VectorXd &joint_velocities, const VectorXd &joint_forces); + + +public: + RobotCoppeliaRosInterface(const ros::NodeHandle& nh, + const std::string& topic_prefix, + const std::vector& jointnames, + const RobotCoppeliaROSConfiguration& configuration, + std::atomic_bool* kill_this_node); + + RobotCoppeliaRosInterface()=delete; + + ~RobotCoppeliaRosInterface(); + VectorXd get_joint_positions() const; + void set_joint_target_positions(const VectorXd& target_joint_positions); + void set_joint_target_velocities(const VectorXd& target_joint_velocities); + int control_loop(); +}; diff --git a/src/robot_coppelia_ros_interface.cpp b/src/robot_coppelia_ros_interface.cpp new file mode 100644 index 0000000..10afcf6 --- /dev/null +++ b/src/robot_coppelia_ros_interface.cpp @@ -0,0 +1,145 @@ +#include "robot_coppelia_ros_interface.h" +#include "std_msgs/Float64MultiArray.h" +#include "sensor_msgs/JointState.h" +#include +#include + +/** + * @brief robot_ros_interface + * @param nh + */ +RobotCoppeliaRosInterface::RobotCoppeliaRosInterface(const ros::NodeHandle& nh, + const std::string& topic_prefix, + const std::vector& jointnames, + const RobotCoppeliaROSConfiguration& configuration, + std::atomic_bool* kill_this_node) + :nh_(nh), + topic_prefix_(topic_prefix), + jointnames_(jointnames), + kill_this_node_(kill_this_node), + configuration_(configuration), + clock_(configuration.thread_sampling_time_nsec) +{ + + //subscriber_joint_state_ = nh_.subscribe(topic_prefix_+ "/get/joint_states", 1, + // &RobotCoppeliaRosInterface::_joint_states_callback, this); + + //publisher_target_joint_positions_ = nh_.advertise + // (topic_prefix_ + "/set/target_joint_positions", 1); + + subscriber_target_joint_positions_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_positions", 1, &RobotCoppeliaRosInterface::_callback_target_joint_positions, this); + publisher_joint_states_ = nh_.advertise(topic_prefix_+ "/get/joint_states", 1); + + vi_ = std::make_shared(); + vi_->connect(configuration_.ip, configuration_.port, 500, 10); + vi_->start_simulation(); +} + +RobotCoppeliaRosInterface::~RobotCoppeliaRosInterface() +{ + vi_->disconnect(); +} + +/** + * @brief get_joint_positions + * @return + */ +VectorXd RobotCoppeliaRosInterface::get_joint_positions() const +{ + return joint_positions_; +} + + +/** + * @brief set_joint_target_positions + * @param target_joint_positions + */ +void RobotCoppeliaRosInterface::set_joint_target_positions(const VectorXd &target_joint_positions) +{ + + std_msgs::Float64MultiArray ros_msg; + ros_msg.data = sas::vectorxd_to_std_vector_double(target_joint_positions); + publisher_target_joint_positions_.publish(ros_msg); + +} + +void RobotCoppeliaRosInterface::set_joint_target_velocities(const VectorXd &target_joint_velocities) +{ + +} + + +void RobotCoppeliaRosInterface::send_joint_states(const VectorXd &joint_positions, const VectorXd &joint_velocities, const VectorXd &joint_forces) +{ + sensor_msgs::JointState ros_msg; + if(joint_positions.size()>0) + ros_msg.position = sas::vectorxd_to_std_vector_double(joint_positions); + if(joint_velocities.size()>0) + ros_msg.velocity = sas::vectorxd_to_std_vector_double(joint_velocities); + if(joint_forces.size()>0) + ros_msg.effort = sas::vectorxd_to_std_vector_double(joint_forces); + publisher_joint_states_.publish(ros_msg); +} + +int RobotCoppeliaRosInterface::control_loop() +{ + try{ + clock_.init(); + ROS_INFO_STREAM(ros::this_node::getName() << "::Waiting to connect with CoppeliaSim..."); + + + while(not _should_shutdown()) + { + clock_.update_and_sleep(); + ros::spinOnce(); + + VectorXd q = vi_->get_joint_positions(jointnames_); + joint_positions_ = q; + send_joint_states(q, VectorXd(), VectorXd()); + + if (target_joint_positions_.size()>0) + { + vi_->set_joint_target_positions(jointnames_, target_joint_positions_); + } + + //auto q = get_joint_positions(); + std::cout<<"q: "<position); +} + +void RobotCoppeliaRosInterface::_callback_target_joint_positions(const std_msgs::Float64MultiArrayConstPtr &msg) +{ + target_joint_positions_ = sas::std_vector_double_to_vectorxd(msg->data); +} diff --git a/src/robot_coppelia_ros_interface_node.cpp b/src/robot_coppelia_ros_interface_node.cpp new file mode 100644 index 0000000..6d72b5d --- /dev/null +++ b/src/robot_coppelia_ros_interface_node.cpp @@ -0,0 +1,115 @@ +/* +# 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 . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 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) +# +# ################################################################ +*/ + + +#include + +#include +#include +//#include +#include +#include +#include +#include "sas_robot_driver_franka.h" +#include "robot_coppelia_ros_interface.h" + +/********************************************* + * SIGNAL HANDLER + * *******************************************/ +#include +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, "robot_coppelia_ros_interface", 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_mode", robot_driver_franka_configuration.mode); + + //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); + + //robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName(); + + //4000000 + + RobotCoppeliaROSConfiguration configuration; + configuration.thread_sampling_time_nsec = 4000000; + configuration.ip = "10.198.113.159"; + configuration.port = 20021; + std::vector jointnames = {"Franka_joint1","Franka_joint2","Franka_joint3", + "Franka_joint4","Franka_joint5","Franka_joint6","Franka_joint7"}; + RobotCoppeliaRosInterface robot_coppelia_ros_interface(nh, "/franka_1_sim",jointnames, configuration, &kill_this_process); + + 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(); + robot_coppelia_ros_interface.control_loop(); + } + catch (const std::exception& e) + { + ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what()); + } + + + return 0; +}