OdometryTracker.h
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <tf/tf.h>
18 #include <nav_msgs/Odometry.h>
19 
20 #ifndef COB_ODOMETRY_TRACKER_H
21 #define COB_ODOMETRY_TRACKER_H
22 
24  nav_msgs::Odometry odom_;
26 public:
27  OdometryTracker(const std::string &from = "odom", const std::string &to = "base_footprint" , double cov_pose = 0.1, double cov_twist = 0.1) {
28  odom_.header.frame_id = from;
29  odom_.child_frame_id = to;
30  for(int i = 0; i < 6; i++){
31  odom_.pose.covariance[i*6+i] = cov_pose;
32  odom_.twist.covariance[6*i+i] = cov_twist;
33  }
34  // odom_.pose.pose.position.z = 0.0;
35  // odom_.twist.twist.linear.z = 0.0;
36  // odom_.twist.twist.angular.x = 0.0;
37  // odom_.twist.twist.angular.y = 0.0;
39  }
40  void init(const ros::Time &now){
41  theta_rob_rad_ = 0;
42 
43  odom_.header.stamp = now;
44 
45  odom_.twist.twist.linear.x = 0;
46  odom_.twist.twist.linear.y = 0;
47  odom_.twist.twist.angular.z = 0;
48 
49  odom_.pose.pose.position.x = 0;
50  odom_.pose.pose.position.y = 0;
51  odom_.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta_rob_rad_);
52 
53  }
54  const nav_msgs::Odometry &getOdometry(){
55  return odom_;
56  }
57  void track(const ros::Time &now, double dt, double vel_x, double vel_y, double vel_theta){
58  // calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom, using now midpoint integration
59 
60  if(dt > 0){
61  odom_.header.stamp = now;
62 
63  double vel_x_mid = (vel_x+odom_.twist.twist.linear.x)/2.0;
64  double vel_y_mid = (vel_y+odom_.twist.twist.linear.y)/2.0;
65 
66  double sin_theta = sin(theta_rob_rad_);
67  double cos_theta = cos(theta_rob_rad_);
68  theta_rob_rad_ += vel_theta * dt;
69 
70  odom_.pose.pose.position.x += (vel_x_mid * cos_theta - vel_y_mid * sin_theta) * dt;
71  odom_.pose.pose.position.y += (vel_x_mid * sin_theta + vel_y_mid * cos_theta) * dt;
72  odom_.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta_rob_rad_);
73 
74  odom_.twist.twist.linear.x = vel_x;
75  odom_.twist.twist.linear.y = vel_y;
76  odom_.twist.twist.angular.z = vel_theta;
77  }
78  }
79 };
80 
81 
82 #endif
nav_msgs::Odometry odom_
const nav_msgs::Odometry & getOdometry()
void init(const ros::Time &now)
void track(const ros::Time &now, double dt, double vel_x, double vel_y, double vel_theta)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
static Time now()
OdometryTracker(const std::string &from="odom", const std::string &to="base_footprint", double cov_pose=0.1, double cov_twist=0.1)


cob_base_controller_utils
Author(s): Felix Messmer , Mathias L├╝dtke
autogenerated on Fri Nov 8 2019 03:46:51