pitch angle from IMU by driving the robot up/down a ramp [closed]

Hello

I trying to get the total angle of a ramp when my robot is driving it up/down of the ramp. I can get the current ramp angle but cant get the total angle of the ramp(when robot start going up/down a ramp till the end of the ramp). I made a Callback but the variable ramp_pitch is not given me the wonted angle. Im posting the code so please any help?

#define RAMP_ANGLE 1.0 //degrees

const double ramp_ang = RAMP_ANGLE * (3.1416/180.0);
double r,p,y, initial_pitch, ramp_pitch, deg_p; tf::Quaternion quat;
ros::Time start_time, end_time;
bool is_first = 1, first_time_rec = 0, end_time_rec = 0;

std::ofstream myfile;

void imuCallback(const sensor_msgs::ImuConstPtr& msg)
{
tf::quaternionMsgToTF(msg->orientation, quat);
tf::Matrix3x3(quat).getRPY(r,p,y);

if(is_first)
{
initial_pitch = p;
is_first = 0;
}
else
{
if(fabs(p-initial_pitch) >= ramp_ang && !first_time_rec)
{
ramp_pitch = (p-initial_pitch);
start_time = ros::Time::now();
first_time_rec = 1;

}
else if((first_time_rec && !end_time_rec) && fabs(p-initial_pitch) <= ramp_ang)
{
end_time = ros::Time::now();
end_time_rec = 1;

}

}

ROS_INFO("roll =  %f, pitch=  %f, ramp_pitch= %f, yaw= %f",r,p*57.3,ramp_pitch*57.3,y);

}

The ramp total angle should be given by ramp_pitch.

Any help?

edit retag reopen merge delete