Ask Your Question
0

AMCL laser skipping scan

asked 2013-12-09 19:54:19 -0500

FuerteNewbie gravatar image

updated 2013-12-10 12:46:29 -0500

image description

I am doing amcl on a quadrotor, as the quadrotor move along and yaw pitch roll might affect the laser scan thus the scan I received might end up confuse my amcl and get the wrong localization. To overcome this problem, my partner has try to compensate laser scan value(within certain range) by creating this node. Let just call it LaserCompensation.

Here's some of the files.

LaserCompensation.cpp

#include <stdexcept>
#include <termios.h>
#include <string>
#include <stdlib.h>
#include <vector>
#include <stdint.h>
#include <stdint.h>
#include <limits>
#include "Rotator.cpp"
#include "sensor_msgs/LaserScan.h"
#include "../../px-ros-pkg/px_comm/msg_gen/cpp/include/px_comm/OpticalFlow.h"
#include "../../flight/msg_gen/cpp/include/flight/flightFeedback.h"
#include <ros/ros.h>
#include <cmath>
#define PI 3.141592654
double Flowtime1 ;
double FlowAltitude1;
double Flowtime2;
double FlowAltitude2;
double altitudedifference;
double timedifference;
double pitchAngle;
double rollAngle;
sensor_msgs::LaserScan receivedscan;
sensor_msgs::LaserScan publishscan;
Rotator *rotator;

void altitudeParse(const px_comm::OpticalFlow::ConstPtr& optFlowMsg)
{
    Flowtime2 = Flowtime1;
    Flowtime1 = optFlowMsg->header.stamp.toSec();
    FlowAltitude2 = FlowAltitude1;
    FlowAltitude1 = optFlowMsg->ground_distance;
    altitudedifference = FlowAltitude1-FlowAltitude2;
    timedifference = Flowtime1 - Flowtime2;
}

void angleParse(const flight::flightFeedback::ConstPtr& msg2)
{
            pitchAngle = msg2->pitchAngle;
            rollAngle = msg2->rollAngle;
}
void Transform()//sensor_msgs::LaserScan& scan_msg)
{

    publishscan.angle_min = -2.35619443;
    publishscan.angle_max = 2.35619443;
    publishscan.angle_increment = (1.25/180*PI);
    publishscan.time_increment = receivedscan.time_increment;
    publishscan.scan_time = receivedscan.scan_time;
    publishscan.range_min = receivedscan.range_min;
    publishscan.range_max = receivedscan.range_max;
    publishscan.header.stamp = receivedscan.header.stamp;
    publishscan.header.frame_id = "test1";
    rotator->set(rollAngle, pitchAngle);
    float x = 0.0;
    float y = 0.0;
    float z = 0.0;
    float xt = 0.0;
    float yt = 0.0;
    float zt = 0.0;
    float r = 0.0;
    float rt = 0.0;
    float indexdifference[216];
    for (int o=0;o<216;o++)
    {
       indexdifference[o]=360.0;
    }

    uint g=0;
    for(int i = 0; i < 1080; i++){
           r = receivedscan.ranges[i];
           x = r*cos(((0.25*i)-45)/180*PI);
           z = r*sin(((0.25*i)-45)/180*PI);
           rotator->unrotate(x, y ,z, xt, yt, zt);
           float d=-90.0;
           if((xt!=0.0)||(zt!=0.0))
           {
               d=atan2(zt,xt)*180.0/PI+45.0;
           }
           if(d<0.0){
               d+=360.0;
           }
           float e= (d*215/270)+0.5;
           int h = e;
           float absolutedifference= std::abs (e-h);
           rt = sqrt(zt*zt+xt*xt);

           if(h>=0.0)
           {
              if(absolutedifference<indexdifference[h])
              {
                 indexdifference[h]=absolutedifference;
                 publishscan.ranges[h]=rt;

              }
           }
           /*if(i<2 && absolutedifference<angle[0])
           {
              angle[0]=d;
              publishscan.ranges[0]=rt; 
           }
           else if(absolutedifference<angle[(i-3/5)+1] && i>2)
           {
              angle[(i-3/5)+1]=d;
              publishscan.ranges[i/5]=rt;
           }*/
    }
}


void parseScan(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
    receivedscan.angle_min = scan_msg->angle_min;
    receivedscan.angle_max = scan_msg->angle_max;
    receivedscan.angle_increment = scan_msg->angle_increment;
    receivedscan.time_increment = scan_msg->time_increment;
    receivedscan.scan_time = scan_msg->scan_time;
    receivedscan.range_min = scan_msg->range_min;
    receivedscan.range_max = scan_msg->range_max;
    receivedscan.ranges = scan_msg->ranges;
    receivedscan.header.stamp = scan_msg->header.stamp;
    receivedscan.header.frame_id = scan_msg->header.frame_id;

    Transform();
}

int main(int argc, char **argv)
   {
     rotator = new Rotator();
     ros::init(argc, argv, "LaserCompensation");
     ros::NodeHandle n;
     publishscan.ranges.resize(216);
     int count = 0;
     ros::Subscriber altitude= n.subscribe( "/px4flow/opt_flow", 1, &altitudeParse ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-12-09 20:35:00 -0500

Jbot gravatar image

updated 2013-12-09 20:37:25 -0500

Try to add in your launch file the remap parameter in your amcl node :

   <node name="amcl" pkg="amcl" type="amcl" output ="screen">
   -> <remap from="scan" to="scan2" />
   <param name="odom_model_type" value="omni"/>

This allow amcl to use scan2 instead of scan topic.

You'll have to do the same thing in your laser_scan_matcher node.

edit flag offensive delete link more

Comments

You mean I have to remap scan2 for laser_scan_matcher too?

FuerteNewbie gravatar imageFuerteNewbie ( 2013-12-18 15:17:12 -0500 )edit

What do you mean by do the same thing in laser scan matcher node? Adding this line into amcl launch code and also laser scan matcher launch code?

FuerteNewbie gravatar imageFuerteNewbie ( 2014-01-01 13:38:16 -0500 )edit

Thanks a lot man it's working! Can explain the theory behind? What have remap actually done?

FuerteNewbie gravatar imageFuerteNewbie ( 2014-01-01 16:15:51 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-12-09 19:54:19 -0500

Seen: 236 times

Last updated: Dec 10 '13