34 #include <sensor_msgs/Imu.h>
35 #include "geometry_msgs/Vector3Stamped.h"
36 #include "geometry_msgs/QuaternionStamped.h"
37 #include "geometry_msgs/Pose.h"
38 #include <sensor_msgs/JointState.h>
40 #include "geometry_msgs/Pose.h"
41 #include "geometry_msgs/Quaternion.h"
42 #include "tf/transform_datatypes.h"
50 #define PFLN {printf("%s %d\n",__FILE__, __LINE__);}
65 t_i_1=msg.header.stamp;
79 int main(
int argc,
char** argv)
81 ros::init(argc, argv,
"atlascar_odometry");
83 ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>(
"/vhc/odometry", 50);
85 tf::TransformBroadcaster odom_broadcaster;
89 Subscriber subscribeVelocityStatusMessages = n.subscribe (
"/vhc/velocity/status", 1,
VelocityMessageHandler);
109 ROS_INFO(
"Starting to spin ...");
118 nav_msgs::Odometry odom;
158 double instant_radious=fabs(l/tan(phi));
161 v_corrected=vl*(instant_radious/(instant_radious-w/2));
164 v_corrected=vl*(instant_radious/(instant_radious+w/2));
178 delta_x=cos(yaw)*cos(phi)*v_corrected*dt;
179 delta_y=sin(yaw)*cos(phi)*v_corrected*dt;
181 delta_yaw=sin(phi)*v_corrected*dt/l;
193 Vector x_estimated=filter.getX();
194 Matrix covariance_matrix =filter.calculateP();
198 geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(x_estimated(3));
219 nav_msgs::Odometry odom;
220 odom.header.stamp =
t_i;
221 odom.header.frame_id =
"/world";
224 odom.pose.pose.position.x = x_estimated(1);
225 odom.pose.pose.position.y = x_estimated(2);
226 odom.pose.pose.position.z = 0.0;
227 odom.pose.pose.orientation = odom_quat;
228 odom.pose.covariance[0]=covariance_matrix(1,1);
229 odom.pose.covariance[7]=covariance_matrix(2,2);
230 odom.pose.covariance[35]=covariance_matrix(3,3);
234 odom.child_frame_id =
"/atc/vehicle/rear_axis";
235 odom.twist.twist.linear.x = vx;
236 odom.twist.twist.linear.y = vy;
237 odom.twist.twist.angular.z = vth;
241 odom_pub.publish(odom);
The documentation of this file is a responsibility of its current developer, Pedro Salvado...
void SetWheelBase(double l_)
atlascar_base::AtlascarVelocityStatus base_velocity
#define DEFAULT_WHEEL_BASE
void VelocityMessageHandler(const atlascar_base::AtlascarVelocityStatus &msg)
non_holonomic_ekfilter::Matrix Matrix
atlascar_base::AtlascarStatus base_status
void SetTimeInterval(double dt_)
void StatusMessageHandler(const atlascar_base::AtlascarStatus &msg)
int main(int argc, char **argv)
non_holonomic_ekfilter::Vector Vector