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

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

Astronaut gravatar image

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

Comments

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

joq gravatar imagejoq ( 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 imageAstronaut ( 2013-05-20 15:16:26 -0500 )edit