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

working coppelia mirror node (#1)

Browse Source 
Reviewed-on: #1
Co-authored-by: QuentinLin <qlin1806@gmail.com>
Co-committed-by: QuentinLin <qlin1806@gmail.com>
This commit was merged in pull request #1.
This commit is contained in:
QuentinLin
2024-08-14 14:27:29 +09:00
committed by qlin
parent 1f842c9ba0
commit d1bc7d5c9f
4 changed files with 290 additions and 292 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -158,6 +158,7 @@ add_dependencies(qros_effector_driver_franka_hand ${catkin_EXPORTED_TARGETS})
add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp)
target_link_libraries(sas_robot_driver_coppelia
dqrobotics
dqrobotics-interface-json11
dqrobotics-interface-vrep)
add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

411
src/coppelia/sas_robot_driver_coppelia.cpp
View File

@@ -4,249 +4,240 @@
namespace sas
{
RobotDriverCoppelia::~RobotDriverCoppelia() {
deinitialize();
disconnect();
}
RobotDriverCoppelia::RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
RobotDriver(break_loops),
RobotDriverCoppelia::RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
configuration_(configuration),
robot_mode_(configuration.robot_mode),
jointnames_(configuration.jointnames),
mirror_mode_(configuration.mirror_mode),
dim_configuration_space_(configuration.jointnames.size()),
real_robot_topic_prefix_(configuration.real_robot_topic_prefix)
clock_(configuration.thread_sampling_time_nsec),
break_loops_(break_loops),
robot_mode_(ControlMode::Position),
vi_(std::make_shared<DQ_VrepInterface>())
{
vi_ = std::make_shared<DQ_VrepInterface>();
desired_joint_velocities_ = VectorXd::Zero(dim_configuration_space_);
auto nodehandle = ros::NodeHandle();
std::cout<<"RobotDriverCoppelia::Rostopic: "<<"/"+real_robot_topic_prefix_<<std::endl;
franka1_ros_ = std::make_shared<sas::RobotDriverInterface>(nodehandle, "/"+real_robot_topic_prefix_);
}
VectorXd RobotDriverCoppelia::get_joint_positions()
// should initialize robot driver interface to real robot
DQ_SerialManipulatorDH smdh = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(configuration_.robot_parameter_file_path);
joint_limits_ = {smdh.get_lower_q_limit(),smdh.get_upper_q_limit()};
if(configuration_.using_real_robot)
{
return current_joint_positions_;
}
void RobotDriverCoppelia::set_target_joint_positions(const VectorXd &desired_joint_positions_rad)
{
desired_joint_positions_ = desired_joint_positions_rad;
}
VectorXd RobotDriverCoppelia::get_joint_velocities()
{
return current_joint_velocities_;
}
void RobotDriverCoppelia::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
{
desired_joint_velocities_ = desired_joint_velocities;
}
VectorXd RobotDriverCoppelia::get_joint_forces()
{
return current_joint_forces_;
}
RobotDriverCoppelia::~RobotDriverCoppelia()=default;
void RobotDriverCoppelia::connect()
{
vi_->connect(configuration_.ip, configuration_.port, 500, 10);
vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_));
std::cout<<"RobotDriverCoppelia::Connecting..."<<std::endl;
}
void RobotDriverCoppelia::disconnect()
{
vi_->disconnect();
if (joint_velocity_control_mode_thread_.joinable())
{
joint_velocity_control_mode_thread_.join();
}
if (joint_velocity_control_mirror_mode_thread_.joinable())
{
joint_velocity_control_mirror_mode_thread_.join();
}
std::cout<<"RobotDriverCoppelia::Disconnected."<<std::endl;
}
void RobotDriverCoppelia::initialize()
{
vi_->start_simulation();
if (mirror_mode_ == false)
{
_start_joint_velocity_control_thread();
ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using real robot, Instantiating robot interface to real driver.");
real_robot_interface_ = std::make_shared<RobotDriverInterface>(nh, configuration_.robot_topic_prefix);
}else{
_start_joint_velocity_control_mirror_thread();
}
std::cout<<"RobotDriverCoppelia::Velocity loop running..."<<std::endl;
}
void RobotDriverCoppelia::deinitialize()
{
vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_));
vi_->stop_simulation();
finish_motion();
std::cout<<"RobotDriverCoppelia::Deinitialized."<<std::endl;
}
void RobotDriverCoppelia::_update_robot_state(const VectorXd &q, const VectorXd &q_dot, const VectorXd &forces)
{
current_joint_positions_ = q;
current_joint_velocities_ = q_dot;
current_joint_forces_ = forces;
}
void RobotDriverCoppelia::finish_motion()
{
for (int i=0;i<1000;i++)
{
set_target_joint_positions(current_joint_positions_);
set_target_joint_velocities(VectorXd::Zero(dim_configuration_space_));
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
finish_motion_ = true;
}
void RobotDriverCoppelia::_start_joint_velocity_control_mode()
{
try{
finish_motion_ = false;
VectorXd q = vi_->get_joint_positions(jointnames_);
VectorXd q_dot = vi_->get_joint_velocities(jointnames_);
VectorXd forces = vi_->get_joint_torques(jointnames_);
_update_robot_state(q, q_dot, forces);
desired_joint_positions_ = q;
while(true)
{
VectorXd q = vi_->get_joint_positions(jointnames_);
VectorXd q_dot = vi_->get_joint_velocities(jointnames_);
VectorXd forces = vi_->get_joint_torques(jointnames_);
_update_robot_state(q, q_dot, forces);
if (robot_mode_ == std::string("VelocityControl"))
{
vi_->set_joint_target_velocities(jointnames_, desired_joint_velocities_);
}
else if (robot_mode_ == std::string("PositionControl"))
{
vi_->set_joint_target_positions(jointnames_, desired_joint_positions_);
}
if (finish_motion_) {
finish_motion_ = false;
return;
ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using simulation robot, simulation mode is set to "+ configuration_.robot_mode);
robot_provider_ = std::make_shared<RobotDriverProvider>(nh, configuration_.robot_topic_prefix);
std::string _mode_upper = configuration_.robot_mode;
std::transform(_mode_upper.begin(), _mode_upper.end(), _mode_upper.begin(), ::toupper);
if(_mode_upper == "POSITIONCONTROL"){
robot_mode_ = ControlMode::Position;
}else if(_mode_upper == "VELOCITYCONTROL"){
robot_mode_ = ControlMode::Velocity;
}else{
throw std::invalid_argument("[" + ros::this_node::getName() + "]::Robot mode must be either 'position' or 'velocity'");
}
}
}
catch(const std::exception& e)
{
std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " << e.what()<<std::endl;
}
catch(...)
{
std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " <<std::endl;
}
}
void RobotDriverCoppelia::_start_joint_velocity_control_thread()
{
finish_motion_ = false;
if (joint_velocity_control_mode_thread_.joinable())
{
joint_velocity_control_mode_thread_.join();
void RobotDriverCoppelia::_update_vrep_position(const VectorXd &joint_positions, const bool& force_update) const{
if(configuration_.vrep_dynamically_enabled){
if(force_update){
vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions);
}
if (joint_velocity_control_mirror_mode_thread_.joinable())
{
joint_velocity_control_mirror_mode_thread_.join();
vi_->set_joint_target_positions(configuration_.vrep_joint_names, joint_positions);
}else{
vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions);
}
joint_velocity_control_mode_thread_ = std::thread(&RobotDriverCoppelia::_start_joint_velocity_control_mode, this);
}
void RobotDriverCoppelia::_start_joint_velocity_control_mirror_thread()
{
finish_motion_ = false;
if (joint_velocity_control_mode_thread_.joinable())
{
joint_velocity_control_mode_thread_.join();
void RobotDriverCoppelia::_update_vrep_velocity(const VectorXd & joint_velocity) const{
if(!configuration_.vrep_dynamically_enabled){
throw std::runtime_error("[RobotDriverCoppelia]::[_update_vrep_velocity]::Vrep is not dynamically enabled");
}
if (joint_velocity_control_mirror_mode_thread_.joinable())
{
joint_velocity_control_mirror_mode_thread_.join();
}
joint_velocity_control_mirror_mode_thread_ = std::thread(&RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode, this);
vi_->set_joint_target_velocities(configuration_.vrep_joint_names, joint_velocity);
}
void RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode()
{
try{
finish_motion_ = false;
std::cout<<"RobotDriverCoppelia::Waiting for real robot topics..."<<std::endl;
VectorXd q;
while (ros::ok()) {
if (franka1_ros_->is_enabled())
{
q = franka1_ros_->get_joint_positions();
break;
}
void RobotDriverCoppelia::_start_control_loop(){
clock_.init();
ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Starting control loop...");
while(!_should_shutdown()){
clock_.update_and_sleep();
ros::spinOnce();
}
std::cout<<"RobotDriverCoppelia::Done!"<<std::endl;
VectorXd q_vrep = vi_->get_joint_positions(jointnames_);
VectorXd q_dot_vrep = vi_->get_joint_velocities(jointnames_);
VectorXd forces_vrep = vi_->get_joint_torques(jointnames_);
_update_robot_state(q_vrep, q_dot_vrep, forces_vrep);
desired_joint_positions_ = q_vrep;
while(ros::ok())
if(!ros::ok()){break_loops_->store(true);}
if(configuration_.using_real_robot){
// real_robot_interface_
auto joint_position = real_robot_interface_->get_joint_positions();
_update_vrep_position(joint_position);
}else{
// robot_provider_
VectorXd target_joint_positions;
auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_joint_names);
VectorXd current_joint_velocity;
if(robot_provider_->is_enabled()) {
if(robot_mode_ == ControlMode::Velocity)
{
q = franka1_ros_->get_joint_positions();
if (q.size() == dim_configuration_space_)
{
VectorXd q_vrep = vi_->get_joint_positions(jointnames_);
VectorXd q_dot_vrep = vi_->get_joint_velocities(jointnames_);
VectorXd forces_vrep = vi_->get_joint_torques(jointnames_);
_update_robot_state(q_vrep, q_dot_vrep, forces_vrep);
if(robot_provider_->is_enabled(sas::RobotDriver::Functionality::VelocityControl)) {
simulated_joint_velocities_ = robot_provider_->get_target_joint_velocities();
current_joint_velocity = simulated_joint_velocities_;
// try{_update_vrep_velocity(simulated_joint_velocities_);}catch (...){}
}
else{
ROS_DEBUG_STREAM("[" + ros::this_node::getName() + "]::Velocity control is not enabled.");
}
target_joint_positions = simulated_joint_positions_;
}else{
target_joint_positions = robot_provider_->get_target_joint_positions();
}
}else {
target_joint_positions = current_joint_positions;
}
_update_vrep_position(target_joint_positions);
robot_provider_->send_joint_states(current_joint_positions, current_joint_velocity, VectorXd());
robot_provider_->send_joint_limits(joint_limits_);
}
}
if (robot_mode_ == std::string("VelocityControl"))
{
vi_->set_joint_target_velocities(jointnames_, gain_*(q-q_vrep));
}
else if (robot_mode_ == std::string("PositionControl"))
{
vi_->set_joint_target_positions(jointnames_, q);
}
if (finish_motion_) {
finish_motion_ = false;
return;
}
}
}
int RobotDriverCoppelia::start_control_loop(){
try{
ROS_INFO_STREAM(ros::this_node::getName() << "::Waiting to connect with coppelia...");
connect();
ROS_INFO_STREAM(ros::this_node::getName() << "::Connected to coppelia.");
ROS_INFO_STREAM(ros::this_node::getName() << "::Initializing...");
initialize();
ROS_INFO_STREAM(ros::this_node::getName() << "::initialized.");
_start_control_loop();
}
catch(const std::exception& e)
{
std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " << e.what()<<std::endl;
ROS_WARN_STREAM(ros::this_node::getName() + "::Error or exception caught::" << e.what());
}
catch(...)
{
std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " <<std::endl;
ROS_WARN_STREAM(ros::this_node::getName() + "::Unexpected error or exception caught");
}
ROS_INFO_STREAM(ros::this_node::getName() << "::Deinitializing...");
deinitialize();
ROS_INFO_STREAM(ros::this_node::getName() << "::deinitialized.");
ROS_INFO_STREAM(ros::this_node::getName() << "::Disconnecting from coppelia...");
disconnect();
ROS_INFO_STREAM(ros::this_node::getName() << "::Disconnected from coppelia.");
return 0;
}
void RobotDriverCoppelia::connect(){
auto ret = vi_->connect(configuration_.vrep_ip, configuration_.vrep_port, 100, 10);
if(!ret){
throw std::runtime_error("[RobotDriverCoppelia]::connect::Could not connect to Vrep");
}
ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::connect::Connected to Vrep");
}
void RobotDriverCoppelia::disconnect(){
vi_->disconnect_all();
}
void RobotDriverCoppelia::initialize(){
if(configuration_.using_real_robot)
{
ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize...");
ros::spinOnce();
int count = 0;
while (!real_robot_interface_->is_enabled()) {
ros::spinOnce();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
count++;
if(count > REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT){
ROS_ERROR_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized. Exiting on TIMEOUT...");
throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized.");
}
if(!ros::ok()) {
ROS_WARN_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved. Exiting...");
throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved, not OK.");
}
}
ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface initialized.");
joint_limits_ = real_robot_interface_->get_joint_limits();
_update_vrep_position(real_robot_interface_->get_joint_positions(), true);
}else{
ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Simulation mode.");
// initialization information for robot driver provider
/**
* TODO: check for making sure real robot is not actually connected
*/
auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_joint_names);
VectorXd current_joint_velocity;
if(robot_mode_ == ControlMode::Velocity)
{current_joint_velocity = VectorXd::Zero(current_joint_positions.size());}
robot_provider_->send_joint_states(current_joint_positions, current_joint_velocity, VectorXd());
robot_provider_->send_joint_limits(joint_limits_);
// start velocity control simulation thread if needed
if(robot_mode_ == ControlMode::Velocity)
{
simulated_joint_positions_ = current_joint_positions;
simulated_joint_velocities_ = current_joint_velocity;
start_simulation_thread();
}
}
}
void RobotDriverCoppelia::deinitialize(){
// nothing to do
}
void RobotDriverCoppelia::start_simulation_thread(){ // thread entry point
if(simulation_thread_started_){
throw std::runtime_error("[RobotDriverCoppelia]::start_simulation_thread::Simulation thread already started");
}
if(velocity_control_simulation_thread_.joinable()){
velocity_control_simulation_thread_.join();
}
velocity_control_simulation_thread_ = std::thread(&RobotDriverCoppelia::_velocity_control_simulation_thread_main, this);
}
void RobotDriverCoppelia::_velocity_control_simulation_thread_main(){
/**
* This thread should not access vrep
*/
simulation_thread_started_ = true;
try{
ROS_INFO_STREAM("[" + ros::this_node::getName() +
"]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Simulation thread started.");
sas::Clock clock = sas::Clock(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC);
double tau = static_cast<double>(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC) * 1e-9;
auto current_joint_positions = simulated_joint_positions_;
clock.init();
while (!(*break_loops_) && ros::ok()) {
current_joint_positions += tau * simulated_joint_velocities_; // no dynamic model
// cap joint limit
auto q_min = std::get<0>(joint_limits_);
auto q_max = std::get<1>(joint_limits_);
for (int i = 0; i < current_joint_positions.size(); i++) {
if (current_joint_positions(i) < q_min(i)) {
current_joint_positions(i) = q_min(i);
}
if (current_joint_positions(i) > q_max(i)) {
current_joint_positions(i) = q_max(i);
}
}
simulated_joint_positions_ = current_joint_positions;
clock.update_and_sleep();
}
}catch(std::exception &e){
ROS_ERROR_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::" + e.what());
simulation_thread_started_ = false;
}catch(...){
ROS_ERROR_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::Unknown");
simulation_thread_started_ = false;
}
}

115
src/coppelia/sas_robot_driver_coppelia.h
View File

@@ -39,10 +39,18 @@
#include <memory>
#include <dqrobotics/DQ.h>
#include <sas_robot_driver/sas_robot_driver.h>
#include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
#include <sas_clock/sas_clock.h>
// #include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
// #include <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
#include <thread>
#include <atomic>
#include <sas_robot_driver/sas_robot_driver_interface.h>
#include <sas_robot_driver/sas_robot_driver_provider.h>
#define VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC 2000000 // 2ms, 500Hz
#define REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT 500
using namespace DQ_robotics;
using namespace Eigen;
@@ -54,81 +62,78 @@ namespace sas
struct RobotDriverCoppeliaConfiguration
{
int thread_sampling_time_nsec;
int port;
std::string ip;
std::vector<std::string> jointnames;
int thread_sampling_time_nsec; // frontend vrep update rate
int vrep_port;
std::string vrep_ip;
std::vector<std::string> vrep_joint_names;
bool vrep_dynamically_enabled = false;
std::string robot_mode;
bool mirror_mode;
std::string real_robot_topic_prefix;
bool using_real_robot;
std::string robot_topic_prefix;
std::string robot_parameter_file_path;
// VectorXd q_min;
// VectorXd q_max;
};
class RobotDriverCoppelia: public RobotDriver
class RobotDriverCoppelia
{
private:
enum ControlMode{
Position=0,
Velocity
};
RobotDriverCoppeliaConfiguration configuration_;
std::string robot_mode_ = std::string("VelocityControl"); // PositionControl
bool mirror_mode_ = false;
double gain_ = 3.0;
std::string real_robot_topic_prefix_;
Clock clock_;
std::atomic_bool* break_loops_;
bool _should_shutdown() const {return (*break_loops_);}
ControlMode robot_mode_;
VectorXd current_joint_positions_;
VectorXd current_joint_velocities_;
VectorXd current_joint_forces_;
VectorXd desired_joint_velocities_;
VectorXd desired_joint_positions_;
std::atomic<bool> finish_motion_;
int dim_configuration_space_;
void _update_robot_state(const VectorXd& q, const VectorXd& q_dot, const VectorXd& forces);
void finish_motion();
void _start_joint_velocity_control_mode();
std::thread joint_velocity_control_mode_thread_;
void _start_joint_velocity_control_thread();
void _start_joint_velocity_control_mirror_mode();
std::thread joint_velocity_control_mirror_mode_thread_;
void _start_joint_velocity_control_mirror_thread();
std::shared_ptr<sas::RobotDriverInterface> franka1_ros_;
protected:
std::shared_ptr<DQ_VrepInterface> vi_;
std::vector<std::string> jointnames_;
std::shared_ptr<sas::RobotDriverInterface> real_robot_interface_ = nullptr;
std::shared_ptr<sas::RobotDriverProvider> robot_provider_ = nullptr;
// backend thread for simulaton
/**
* Current simulation mechanism is not accounting for any robot dynamics, just the joint limits
*/
std::thread velocity_control_simulation_thread_;
VectorXd simulated_joint_positions_;
VectorXd simulated_joint_velocities_;
void start_simulation_thread(); // thread entry point
void _velocity_control_simulation_thread_main();
std::atomic_bool simulation_thread_started_{false};
bool initialized_ = false;
inline void _assert_initialized() const{
if (!initialized_){throw std::runtime_error("[RobotDriverCoppelia] Robot driver not initialized");}
}
inline void _assert_is_alive() const{
if(!configuration_.using_real_robot && !simulation_thread_started_){throw std::runtime_error("[RobotDriverCoppelia] Robot Simulation is not alive");}
}
void _start_control_loop();
protected:
inline void _update_vrep_position(const VectorXd &joint_positions, const bool& force_update=false) const;
inline void _update_vrep_velocity(const VectorXd & joint_velocity) const;
public:
RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;
RobotDriverCoppelia()=delete;
~RobotDriverCoppelia();
RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
int start_control_loop(); // public entry point
VectorXd get_joint_positions() override;
void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override;
void connect();
void disconnect();
VectorXd get_joint_velocities() override;
void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override;
VectorXd get_joint_forces() override;
void connect() override;
void disconnect() override;
void initialize() override;
void deinitialize() override;
void initialize();
void deinitialize();
std::tuple<VectorXd, VectorXd> joint_limits_;
};
}

39
src/sas_robot_driver_coppelia_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 (qlin1806@g.ecc.u-tokyo.ac.jp)
-Adapted for simplify operation
#
# ################################################################
*/
@@ -52,6 +54,7 @@ void sig_int_handler(int)
}
int main(int argc, char **argv)
{
if(signal(SIGINT, sig_int_handler) == SIG_ERR)
@@ -61,39 +64,37 @@ int main(int argc, char **argv)
ros::init(argc, argv, "sas_robot_driver_coppelia_node", ros::init_options::NoSigintHandler);
ROS_WARN("=====================================================================");
ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------");
ROS_WARN("--------------------- Quentin Lin -----------------------------------");
ROS_WARN("=====================================================================");
ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
ros::NodeHandle nh;
sas::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration;
sas::get_ros_param(nh,"/robot_ip_address",robot_driver_coppelia_configuration.ip);
sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_coppelia_configuration.thread_sampling_time_nsec);
sas::get_ros_param(nh,"/vrep_ip",robot_driver_coppelia_configuration.vrep_ip);
sas::get_ros_param(nh,"/vrep_port",robot_driver_coppelia_configuration.vrep_port);
sas::get_ros_param(nh,"/vrep_joint_names", robot_driver_coppelia_configuration.vrep_joint_names);
sas::get_ros_param(nh,"/vrep_dynamically_enabled", robot_driver_coppelia_configuration.vrep_dynamically_enabled);
sas::get_ros_param(nh,"/robot_mode", robot_driver_coppelia_configuration.robot_mode);
sas::get_ros_param(nh,"/vrep_port", robot_driver_coppelia_configuration.port);
sas::get_ros_param(nh,"/vrep_robot_joint_names", robot_driver_coppelia_configuration.jointnames);
sas::get_ros_param(nh,"/mirror_mode", robot_driver_coppelia_configuration.mirror_mode);
sas::get_ros_param(nh, "/real_robot_topic_prefix", robot_driver_coppelia_configuration.real_robot_topic_prefix);
sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
sas::get_ros_param(nh,"/using_real_robot", robot_driver_coppelia_configuration.using_real_robot);
sas::get_ros_param(nh,"/robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix);
sas::get_ros_param(nh,"/robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);
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);
// std::vector<double> q_min_vec, q_max_vec;
// sas::get_ros_param(nh,"/q_min", q_min_vec);
// robot_driver_coppelia_configuration.q_min = sas::std_vector_double_to_vectorxd(q_min_vec);
// sas::get_ros_param(nh,"/q_max", q_max_vec);
// robot_driver_coppelia_configuration.q_max = sas::std_vector_double_to_vectorxd(q_max_vec);
robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
try
{
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Coppelia robot.");
sas::RobotDriverCoppelia robot_driver_coppelia(robot_driver_coppelia_configuration,
sas::RobotDriverCoppelia robot_driver_coppelia(nh, robot_driver_coppelia_configuration,
&kill_this_process);
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
sas::RobotDriverROS robot_driver_ros(nh,
&robot_driver_coppelia,
robot_driver_ros_configuration,
&kill_this_process);
robot_driver_ros.control_loop();
return robot_driver_coppelia.start_control_loop();
}
catch (const std::exception& e)
{