31 #include <sensor_msgs/Imu.h>
33 #include <tf/transform_listener.h>
34 #include <nav_msgs/Odometry.h>
37 #include <std_msgs/Float64.h>
79 int main(
int argc,
char** argv)
82 ros::init(argc, argv,
"imu_egomotion");
85 Subscriber imu= n.subscribe(
"/atc/imu/data",1,
ImuCallback);
89 Publisher imu_norm=n.advertise<std_msgs::Float64>(
"/imu_norm",50);
90 Publisher accel_tan=n.advertise<std_msgs::Float64>(
"/accel_tan",50);
91 Publisher accel_cent=n.advertise<std_msgs::Float64>(
"/accel_cent",50);
95 ROS_INFO(
"Starting to spin ...");
101 ROS_ERROR(
"Message type changed, AtlascarStatus and AtlascarVelocityStatus no longer avaliable!! please correct");
134 double instant_radious=fabs(l/tan(phi));
137 v_corrected=vl*(instant_radious/(instant_radious-w/2));
139 v_corrected=vl*(instant_radious/(instant_radious+w/2));
141 cout <<
"vl_ant" <<
v_ant <<
"\n"<< endl;
142 cout <<
"vl" << vl <<
"\n"<< endl;
143 cout <<
"vl_corre" << v_corrected <<
"\n"<< endl;
147 double acc_cent=(v_corrected*v_corrected)/instant_radious;
int main(int argc, char **argv)
nav_msgs::Odometry odometry_
void ImuCallback(const sensor_msgs::Imu::ConstPtr &msg)
sensor_msgs::Imu imu_data
ros::Publisher * p_pitch_pub
std_msgs::Float64 acc_norm_imu
std_msgs::Float64 cent_msg
std_msgs::Float64 tan_msg