vpROSRobotFrankaCoppeliasim.h

Go to the documentation of this file.

/*
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2021 by INRIA. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Franka robot ROS simulator coupled with Coppeliasim.
 *
 * Authors:
 * Alexander Oliva
 * Fabien Spindler
 */

#ifndef vpROSRobotFrankaCoppeliasim_h
#define vpROSRobotFrankaCoppeliasim_h

#include <ros/ros.h>

#include <geometry_msgs/Inertia.h>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>

#include <visp3/core/vpConfig.h>

#include <visp_ros/vpRobotFrankaSim.h>

class VISP_EXPORT vpROSRobotFrankaCoppeliasim : public vpRobotFrankaSim
{
public:
  vpROSRobotFrankaCoppeliasim();
  virtual ~vpROSRobotFrankaCoppeliasim();

  void connect( const std::string &robot_ID = "");

  void coppeliasimPauseSimulation( double sleep_ms = 1000. );
  void coppeliasimStartSimulation( double sleep_ms = 1000. );
  void coppeliasimStopSimulation( double sleep_ms = 1000. );
  void coppeliasimTriggerNextStep();

  double getCoppeliasimSimulationTime();
  bool getCoppeliasimSimulationStepDone();
  int getCoppeliasimSimulationState();

  inline bool getCoppeliasimSyncMode() { return m_syncModeEnabled; }

  inline bool isConnected() const { return m_connected; }

  void setCoppeliasimSyncMode( bool enable, double sleep_ms = 1000. );

  void setPosition( const vpRobot::vpControlFrameType frame,
                            const vpColVector &position );

  vpRobot::vpRobotStateType setRobotState( vpRobot::vpRobotStateType newState );

  inline void setTopicJointState( const std::string &topic_jointState )
  {
        m_topic_jointState = topic_jointState;
  }

  inline void setTopicJointStateCmd( const std::string &topic_jointStateCmd )
  {
    m_topic_jointStateCmd = topic_jointStateCmd;
  }

  inline void setTopicRobotStateCmd( const std::string &topic_robotState )
  {
        m_topic_robotStateCmd = topic_robotState;
  }

  inline void setTopic_g0( const std::string &topic_g0 ) { m_topic_g0 = topic_g0; }

  inline void setTopic_eMc( const std::string &topic_eMc ) { m_topic_eMc = topic_eMc; }

  inline void setTopic_flMe( const std::string &topic_flMe ) { m_topic_flMe = topic_flMe; }

  inline void setTopic_flMcom( const std::string &topic_flMcom ) { m_topic_flMcom = topic_flMcom; }

  inline void setTopic_toolInertia( const std::string &topic_toolInertia ) { m_topic_toolInertia = topic_toolInertia; }

  void setCoppeliasimSimulationStepDone( bool simulationStepDone );

  void wait( double timestamp_second, double duration_second );

protected:
  void callbackJointState( const sensor_msgs::JointState &joint_state );
  void callback_g0( const geometry_msgs::Vector3 &g0_msg );
  void callback_eMc( const geometry_msgs::Pose &pose_msg );
  void callback_flMe( const geometry_msgs::Pose &pose_msg );
  void callback_toolInertia( const geometry_msgs::Inertia &inertia_msg );
  void callbackSimulationStepDone( const std_msgs::Bool &msg );
  void callbackSimulationTime( const std_msgs::Float32 &msg );
  void callbackSimulationState( const std_msgs::Int32 &msg );

  void readingLoop();

  void positionControlLoop();
  void torqueControlLoop();
  void velocityControlLoop();

  std::thread m_controlThread;
  std::thread m_acquisitionThread;

  // Subscribed topics updated by Coppeliasim
  std::string m_topic_jointState;
  std::string m_topic_g0;
  std::string m_topic_eMc;
  std::string m_topic_flMe;
  std::string m_topic_flMcom;
  std::string m_topic_toolInertia;
  // Published topics to update Coppeliasim
  std::string m_topic_jointStateCmd;
  std::string m_topic_robotStateCmd;

  bool m_connected;

  // Position controller
  std::atomic_bool m_posControlThreadIsRunning;
  std::atomic_bool m_posControlThreadStopAsked;
  std::atomic_bool m_posControlLock;
  std::atomic_bool m_posControlNewCmd;

  // Velocity controller
  std::atomic_bool m_velControlThreadIsRunning;
  std::atomic_bool m_velControlThreadStopAsked;

  // Force/torque controller
  std::atomic_bool m_ftControlThreadIsRunning;
  std::atomic_bool m_ftControlThreadStopAsked;

  // Publisher
  ros::Publisher m_pub_jointStateCmd;
  ros::Publisher m_pub_robotStateCmd;
  ros::Publisher m_pub_startSimulation;
  ros::Publisher m_pub_pauseSimulation;
  ros::Publisher m_pub_stopSimulation;
  ros::Publisher m_pub_triggerNextStep;
  ros::Publisher m_pub_enableSyncMode;

  // Subscriber
  ros::Subscriber m_sub_coppeliasim_jointState;
  ros::Subscriber m_sub_coppeliasim_g0;
  ros::Subscriber m_sub_coppeliasim_eMc;
  ros::Subscriber m_sub_coppeliasim_flMe;
  ros::Subscriber m_sub_coppeliasim_toolInertia;
  ros::Subscriber m_sub_coppeliasim_simulationStepDone;
  ros::Subscriber m_sub_coppeliasim_simulationTime;
  ros::Subscriber m_sub_coppeliasim_simulationState;

  // Simulation
  bool m_simulationStepDone;
  float m_simulationTime;
  bool m_syncModeEnabled;
  int m_simulationState;

  bool m_overwrite_toolInertia; // Flag to indicate that the inertia parameters of the tool should no more be updated
                                // from topic
  bool m_overwrite_flMe;        // Flag to indicate that flMe should no more be updated from topic
};

#endif