49 #include <visp3/core/vpHomogeneousMatrix.h> 50 #include <visp3/robot/vpRobotException.h> 67 , _master_uri(
"http://127.0.0.1:11311" )
68 , _topic_cmd(
"/RosAria/cmd_vel" )
69 , _topic_odom(
"odom" )
117 throw( vpRobotException( vpRobotException::constructionError,
125 argv[0] =
new char[255];
126 argv[1] =
new char[255];
128 std::string exe =
"ros.exe", arg1 =
"__master:=";
129 strcpy( argv[0], exe.c_str() );
131 strcpy( argv[1], arg1.c_str() );
151 geometry_msgs::Twist msg;
152 if ( frame == vpRobot::REFERENCE_FRAME )
154 msg.linear.x = vel[0];
155 msg.linear.y = vel[1];
156 msg.linear.z = vel[2];
157 msg.angular.x = vel[3];
158 msg.angular.y = vel[4];
159 msg.angular.z = vel[5];
164 throw vpRobotException( vpRobotException::wrongStateError,
165 "Cannot send the robot velocity in the specified control frame" );
185 if ( frame == vpRobot::REFERENCE_FRAME )
191 vpRotationMatrix R(
q );
199 throw vpRobotException( vpRobotException::wrongStateError,
200 "Cannot get the robot position in the specified control frame" );
225 timestamp.tv_sec =
_sec;
226 timestamp.tv_nsec =
_nsec;
228 if ( frame == vpRobot::REFERENCE_FRAME )
231 pose_prev = pose_cur;
235 throw vpRobotException( vpRobotException::wrongStateError,
236 "Cannot get robot displacement in the specified control frame" );
254 struct timespec timestamp;
264 p.set( msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z );
265 q.
set( msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z,
266 msg->pose.pose.orientation.w );
270 double dt = ( (double)msg->header.stamp.sec - (
double)
_sec ) +
271 ( (
double)msg->header.stamp.nsec - (double)
_nsec ) / 1000000000.0;
279 _sec = msg->header.stamp.sec;
280 _nsec = msg->header.stamp.nsec;
287 geometry_msgs::Twist msg;
ROSCPP_DECL const std::string & getURI()
void setCmdVelTopic(std::string topic_name)
void set(const double x, const double y, const double z, const double w)
void getPosition(const vpRobot::vpControlFrameType, vpColVector &)
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::AsyncSpinner * spinner
ROSCPP_DECL bool isInitialized()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
vpRobot implementation for ROS middleware.
void setNodespace(std::string nodespace)
void publish(const boost::shared_ptr< M > &message) const
virtual ~vpROSRobot()
destructor
void setOdomTopic(std::string topic_name)
void init()
basic initialization
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void getDisplacement(const vpRobot::vpControlFrameType, vpColVector &)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setMasterURI(std::string master_uri)