test-vel.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2021 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  *****************************************************************************/
32 
34 
35 #include <iostream>
36 #include <mutex>
37 
39 
40 static bool s_simStepDone = true;
41 static std::mutex s_mutex_ros;
42 static float s_simTime = 0;
43 
44 void
45 simStepDone_callback( const std_msgs::Bool &msg )
46 {
47  std::lock_guard< std::mutex > lock( s_mutex_ros );
48  s_simStepDone = msg.data;
49 }
50 
51 void
52 simTime_callback( const std_msgs::Float32 &msg )
53 {
54  std::lock_guard< std::mutex > lock( s_mutex_ros );
55  s_simTime = msg.data;
56 }
57 
58 int
59 main( int argc, char **argv )
60 {
61 
62  try
63  {
64  //------------------------------------------------------------------------//
65  //------------------------------------------------------------------------//
66  // ROS node
67  ros::init( argc, argv, "visp_ros" );
68  ros::NodeHandlePtr n = boost::make_shared< ros::NodeHandle >();
69  ros::Rate loop_rate( 1000 );
70  ros::spinOnce();
71 
73  robot.setVerbose( true );
74  robot.setTopicJointState( "/vrep/franka/joint_state" );
75  robot.setTopic_eMc( "/vrep/franka/eMc" );
76  robot.connect();
77 
78  ros::Publisher enableSyncMode_pub = n->advertise< std_msgs::Bool >( "/enableSyncMode", 1 );
79  ros::Publisher startSimTrigger_pub = n->advertise< std_msgs::Bool >( "/startSimulation", 1 );
80  ros::Publisher stopSimTrigger_pub = n->advertise< std_msgs::Bool >( "/stopSimulation", 1 );
81  std_msgs::Bool trigger;
82  std_msgs::Bool syncMode;
83  std_msgs::Bool startStopSim;
84 
85  std::string simulationStepDone_topic_name = "/simulationStepDone";
86  std::cout << "Subscribe to " << simulationStepDone_topic_name << std::endl;
87  ros::Subscriber sub_simStepDone = n->subscribe( simulationStepDone_topic_name, 1, simStepDone_callback );
88  std::string simulationTime_topic_name = "/simulationTime";
89  std::cout << "Subscribe to " << simulationTime_topic_name << std::endl;
90  ros::Subscriber sub_simulationTime = n->subscribe( simulationTime_topic_name, 1, simTime_callback );
91 
92  startStopSim.data = true;
93  startSimTrigger_pub.publish( startStopSim );
94  vpTime::wait( 1000 );
95  stopSimTrigger_pub.publish( startStopSim );
96  vpTime::wait( 1000 );
97  syncMode.data = true;
98  enableSyncMode_pub.publish( trigger );
99  startSimTrigger_pub.publish( startStopSim );
100  vpTime::wait( 1000 );
101 
102  robot.setRobotState( vpRobot::STATE_VELOCITY_CONTROL );
103 
104  double t_start = vpTime::measureTimeSecond();
105  double t_init = vpTime::measureTimeMs();
106  double t_init_prev = t_init;
107 
108  float t_simTime_start = 0, t_simTime = 0, t_simTime_prev = 0;
109  ;
110  s_mutex_ros.lock();
111  t_simTime_start = t_simTime_prev = s_simTime;
112  s_mutex_ros.unlock();
113  t_init_prev = vpTime::measureTimeMs();
114 
115  vpColVector q_init, q_final;
116  robot.getPosition( vpRobot::JOINT_STATE, q_init );
117  std::cout << "q initial: " << 180. / M_PI * q_init.t() << " deg" << std::endl;
118 
119  while ( vpTime::measureTimeSecond() - t_start < 10 )
120  {
121  ros::spinOnce();
122  t_init = vpTime::measureTimeMs();
123  s_mutex_ros.lock();
124  t_simTime = s_simTime;
125  s_mutex_ros.unlock();
126 
127  vpColVector qdot( 7, 0 );
128  qdot[0] = vpMath::rad( 10 );
129  robot.setVelocity( vpRobot::JOINT_STATE, qdot );
130  vpTime::sleepMs( 50 );
131 
132  std::stringstream ss;
133  ss << "Loop time: " << t_init - t_init_prev << " - " << t_simTime - t_simTime_prev << " ms";
134  // std::cout << ss.str() << std::endl;
135  t_init_prev = t_init;
136  t_simTime_prev = t_simTime;
137  }
138  std::cout << "Elapsed time: " << vpTime::measureTimeSecond() - t_start << " seconds - "
139  << t_simTime - t_simTime_start << std::endl;
140 
141  robot.getPosition( vpRobot::JOINT_STATE, q_final );
142  std::cout << "q final: " << 180 / M_PI * q_final.t() << " deg" << std::endl;
143  std::cout << "Joint displacement: " << 180. / M_PI * ( q_final - q_init ).t() << " deg" << std::endl;
144 
145  stopSimTrigger_pub.publish( startStopSim );
146  }
147  catch ( const vpException &e )
148  {
149  std::cout << "ViSP exception: " << e.what() << std::endl;
150  std::cout << "Stop the robot " << std::endl;
151  return EXIT_FAILURE;
152  }
153 
154  return 0;
155 }
ros::Publisher
s_simTime
static float s_simTime
Definition: test-vel.cpp:42
boost::shared_ptr< NodeHandle >
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
vpRobotFrankaSim::getPosition
virtual void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
Definition: vpRobotFrankaSim.cpp:417
vpRobotFrankaSim::setVelocity
virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpRobotFrankaSim.cpp:683
vpROSRobotFrankaCoppeliasim::setTopicJointState
void setTopicJointState(const std::string &topic_jointState)
Definition: vpROSRobotFrankaCoppeliasim.h:130
vpROSRobotFrankaCoppeliasim
Definition: vpROSRobotFrankaCoppeliasim.h:87
ros::spinOnce
ROSCPP_DECL void spinOnce()
vpROSRobotFrankaCoppeliasim::setTopic_eMc
void setTopic_eMc(const std::string &topic_eMc)
Definition: vpROSRobotFrankaCoppeliasim.h:171
simStepDone_callback
void simStepDone_callback(const std_msgs::Bool &msg)
Definition: test-vel.cpp:45
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
vpROSRobotFrankaCoppeliasim::setRobotState
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Definition: vpROSRobotFrankaCoppeliasim.cpp:764
vpRobotFrankaSim::setVerbose
void setVerbose(bool verbose)
Definition: vpRobotFrankaSim.h:111
vpROSRobotFrankaCoppeliasim.h
simTime_callback
void simTime_callback(const std_msgs::Float32 &msg)
Definition: test-vel.cpp:52
main
int main(int argc, char **argv)
Definition: test-vel.cpp:59
s_simStepDone
static bool s_simStepDone
Definition: test-vel.cpp:40
ros::Rate
vpROSRobotFrankaCoppeliasim::connect
void connect(const std::string &robot_ID="")
Definition: vpROSRobotFrankaCoppeliasim.cpp:132
ros::Subscriber
s_mutex_ros
static std::mutex s_mutex_ros
Definition: test-vel.cpp:41


visp_ros
Author(s): Francois Pasteau, Fabien Spindler, Gatien Gaumerais, Alexander Oliva
autogenerated on Wed Mar 2 2022 01:13:33