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

Added a new class

Browse Source
This commit is contained in:
Juancho
2023-08-02 22:19:34 +09:00
parent 87024fb996
commit e45a160fd0
4 changed files with 339 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
src/robot_coppelia_ros_interface.cpp Normal file
View File

@@ -0,0 +1,145 @@
#include "robot_coppelia_ros_interface.h"
#include "std_msgs/Float64MultiArray.h"
#include "sensor_msgs/JointState.h"
#include <sas_common/sas_common.h>
#include <sas_conversions/sas_conversions.h>
/**
* @brief robot_ros_interface
* @param nh
*/
RobotCoppeliaRosInterface::RobotCoppeliaRosInterface(const ros::NodeHandle& nh,
const std::string& topic_prefix,
const std::vector<std::string>& 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<std_msgs::Float64MultiArray>
// (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<sensor_msgs::JointState>(topic_prefix_+ "/get/joint_states", 1);
vi_ = std::make_shared<DQ_VrepInterface>();
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: "<<q.transpose()<<std::endl;
std::cout<<"target q: "<<target_joint_positions_.transpose()<<std::endl;
ros::spinOnce();
}
}
catch(const std::exception& e)
{
ROS_WARN_STREAM(ros::this_node::getName() + "::Error or exception caught::" << e.what());
}
catch(...)
{
ROS_WARN_STREAM(ros::this_node::getName() + "::Unexpected error or exception caught");
}
return 0;
}
/**
* @brief _joint_states_callback
* @param jointstate
*/
bool RobotCoppeliaRosInterface::_should_shutdown() const
{
return (*kill_this_node_);
}
void RobotCoppeliaRosInterface::_joint_states_callback(const sensor_msgs::JointState::ConstPtr& jointstate)
{
joint_positions_ = sas::std_vector_double_to_vectorxd(jointstate->position);
}
void RobotCoppeliaRosInterface::_callback_target_joint_positions(const std_msgs::Float64MultiArrayConstPtr &msg)
{
target_joint_positions_ = sas::std_vector_double_to_vectorxd(msg->data);
}

115
src/robot_coppelia_ros_interface_node.cpp Normal file
View File

@@ -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 <https://www.gnu.org/licenses/>.
#
# ################################################################
#
# 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 <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"
#include "robot_coppelia_ros_interface.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, "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<std::string> 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;
}