48 #include <visp/vpHomogeneousMatrix.h>
49 #include <visp/vpRobotException.h>
51 #include <visp/vpDebug.h>
67 _master_uri(
"http://127.0.0.1:11311"),
68 _topic_cmd(
"/RosAria/cmd_vel"),
95 std::cout <<
"ici 1\n";
97 if(!ros::isInitialized()) ros::init(argc, argv,
"visp_node", ros::init_options::AnonymousName);
98 n =
new ros::NodeHandle;
101 spinner =
new ros::AsyncSpinner(1);
104 std::cout <<
"ici 2\n";
113 if(ros::isInitialized() && ros::master::getURI() !=
_master_uri){
114 throw (vpRobotException(vpRobotException::constructionError,
115 "ROS robot already initialised with a different master_URI (" + ros::master::getURI() +
" != " +
_master_uri +
")") );
120 argv[0] =
new char [255];
121 argv[1] =
new char [255];
123 std::string exe =
"ros.exe", arg1 =
"__master:=";
124 strcpy(argv[0], exe.c_str());
126 strcpy(argv[1], arg1.c_str());
146 geometry_msgs::Twist msg;
147 if (frame == vpRobot::REFERENCE_FRAME)
149 msg.linear.x = vel[0];
150 msg.linear.y = vel[1];
151 msg.linear.z = vel[2];
152 msg.angular.x = vel[3];
153 msg.angular.y = vel[4];
154 msg.angular.z = vel[5];
159 throw vpRobotException (vpRobotException::wrongStateError,
160 "Cannot send the robot velocity in the specified control frame");
178 if (frame == vpRobot::REFERENCE_FRAME)
184 vpRotationMatrix R(
q);
192 throw vpRobotException (vpRobotException::wrongStateError,
193 "Cannot get the robot position in the specified control frame");
215 timestamp.tv_sec =
_sec;
216 timestamp.tv_nsec =
_nsec;
218 if(frame == vpRobot::REFERENCE_FRAME){
220 pose_prev = pose_cur;
224 throw vpRobotException (vpRobotException::wrongStateError,
225 "Cannot get robot displacement in the specified control frame");
240 struct timespec timestamp;
248 p.set(msg->pose.pose.position.x,msg->pose.pose.position.y,msg->pose.pose.position.z);
249 q.set(msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
252 double dt = ((double)msg->header.stamp.sec - (
double)
_sec) + ((
double)msg->header.stamp.nsec - (double)
_nsec) / 1000000000.0;
260 _sec = msg->header.stamp.sec;
261 _nsec = msg->header.stamp.nsec;
267 geometry_msgs::Twist msg;
void setCmdVelTopic(std::string topic_name)
void getPosition(const vpRobot::vpControlFrameType, vpColVector &)
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
ros::AsyncSpinner * spinner
vpRobot implementation for ROS middleware.
void setNodespace(std::string nodespace)
virtual ~vpROSRobot()
destructor
void setOdomTopic(std::string topic_name)
void init()
basic initialization
void getDisplacement(const vpRobot::vpControlFrameType, vpColVector &)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setMasterURI(std::string master_uri)