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

asked 2013-05-19 21:47:35 -0500

Astronaut gravatar image


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);

    initial_pitch = p;
    is_first = 0;
    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 flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2017-06-07 20:28:27.920603


I can't quite make out what you are trying to do here.

joq gravatar image joq  ( 2013-05-20 14:07:37 -0500 )edit

Well I define like ramp angle threshold. And when my platform is going up /down a ramp ( so bigger that abs value of the threshold) start recording the time till the platform is up or down the ramp.

Astronaut gravatar image Astronaut  ( 2013-05-20 15:16:26 -0500 )edit