0
$\begingroup$

Rosanswers logo

Hello,

I have robot_localization setup to fuse IMU data from a flight controller with pose data from ORB-SLAM. I am also using gmapping to generate a 2D map. When I run my robot_localization node, my tf tree becomes odom -> base_link -> camera_link, which I think is what I'm supposed to get because right now I'm assuming that my camera is my base. However, I'm also trying to incorporate a particle filter into this setup. And when I try to run my particle filter, my tf tree changes to map -> base_link and I'm not sure why. I've been trying to find answers but I feel like a lot is going over my head.

In any case, here are the steps I did when I was using robot_localization and also use the particle filter:

  1. Go to a known fixed position (initial point).
  2. Run camera, ORB, robot_localization node, and gmapping. (At this point, my tf tree is odom -> base_link)
  3. Start mapping around and return to initial point when done.
  4. Save the map using map_server.
  5. Close gmapping.
  6. Launch the particle filter and range finders. (This is where my tf tree changes to map -> base_link while my odom frame seems to just float and is unconnected to anything).

Also here is the config file I use for my robot_localization node:

frequency: 50
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

pose0: /orb/pose

pose0_config: [true,  true,  true,      # Position
               true,  true,  true,    # Orientation
               false,  false, false,     # Linear Velocity
               false,  false, false,      # Angular Velocity
               false,  false, false]     # Linear Acceleration   

pose0_queue_size: 2

pose0_nodelay: false

pose0_differential: false

pose0_relative: false

imu0: /mavros/imu/data

imu0_config: [true, true, true,    # Position
              true, true, true,     # Orientation
              false, false, false,    # Linear Velocity
              true, true, true,     # Angular Velocity
              true, true, true]     # Linear Acceleration              
            
imu0_nodelay: false
imu0_differential: true
imu0_relative: false
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #

imu0_remove_gravitational_acceleration: false

use_control: true
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 1.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 1.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.5, 0.0, 0.0, 0.0, 1.5]
deceleration_gains: [1.0, 0.7, 0.0, 0.0, 0.0, 1.0]

process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

initial_estimate_covariance: [1e-6, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1e-6, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-5,  0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-5,  0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-6, 0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-6, 0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

EDIT: Robot setup specifics based on tuandl's comments

Our robot setup is as follows:

  • We are using the r200 realsense camera to extract depth data and turn it into laserscan using the depth_to_laserscan node
  • With this, we are using the ORB-SLAM package to get pose data and combine it with our flight controller IMU data
  • We use the robot_localization package to fuse the ORB-SLAM pose with the flight controller IMU and our config file is what I have above. This resulting odom data is our odom->base transform
  • Next we run gmapping to construct our 2d map and save it using map_server
  • We then load this map into our custom BFL particle filter as well as range data from 3 range sensors attached to our robot. The resulting pose is our map->base transform.

Right now we are having trouble creating our map->odom transform in our particle filter, and we are trying to find some reference code such as the amcl code.

EDIT2: Here is the code we currently have for our custom particle filter. Most of this is taken directly from the BFL tutorial on how to write a custom filter. The parts that we added relate to gathering the data from our EKF and range finders:

#include <filter/bootstrapfilter.h>
#include <model/systemmodel.h>
#include <model/measurementmodel.h>
#include "nonlinearSystemPdf.h"
#include "nonlinearMeasurementPdf.h"
#include <iostream>
#include <fstream>
// Include file with properties
#include "constants.h"
#include "customparticlefilter.h"
#include <ros/ros.h>
#include <string>
#include <std_msgs/Empty.h>

//Need messages types for linear velocities and range measures
#include <ardrone_autonomy/Navdata.h>
//#include <ardrone_autonomy/Ranges.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Vector3.h>

#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include "nav_msgs/GetMap.h"
#include "map/map.h"

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include "tf/tf.h"

using namespace MatrixWrapper;
using namespace BFL;
using namespace std;

/*
 The necessary SYSTEM MODEL is:
 x_k      = x_{k-1} + v_{k-1} * cos(theta) * delta_t
 y_k      = y_{k-1} + v_{k-1} * sin(theta) * delta_t
*/

class ParticleFilterNode
{
   ros::NodeHandle nh_;
   ros::Subscriber navi_sub;
   ros::Subscriber sensor_sub;
   ros::Publisher pose_pub;
   ros::Publisher particle_pub;
   map_t* map_;
   NonlinearSystemPdf *sys_pdf;
   SystemModel<ColumnVector> *sys_model;
   NonlinearMeasurementPdf *meas_pdf;
   MeasurementModel<ColumnVector,ColumnVector> *meas_model;
   MCPdf<ColumnVector> *prior_discr;
   CustomParticleFilter *filter;
   ros::Time prevNavDataTime;
   double dt;

   tf::TransformBroadcaster br;
   tf::TransformBroadcaster* tfb_;
   tf::TransformListener tf_listener;
   tf::Transform transform;

   tf::StampedTransform odom_to_base_link;
   tf::StampedTransform map_to_base_link;
   tf::Transform latest_tf_;

   struct TransformListenerWrapper : public tf::TransformListener
   {
      inline tf2_ros::Buffer &getBuffer() {return tf2_buffer_;}
   };

   TransformListenerWrapper* tf_;

   ros::Duration transform_tolerance_;

public:
   ParticleFilterNode()
   {
       navi_sub = nh_.subscribe("/odometry/filtered", 1, &ParticleFilterNode::InputCb, this);
       sensor_sub = nh_.subscribe("/lidarArray", 1, &ParticleFilterNode::MeasurementCb, this);
       pose_pub = nh_.advertise<geometry_msgs::PoseStamped>("/pose_pf",1);
       particle_pub = nh_.advertise<geometry_msgs::PoseArray>("/particlecloud",1);
       dt = 0.0;

       sys_model = NULL;
       meas_model = NULL;
       filter = NULL;
       map_ = NULL;
       requestMap();

   }

   ~ParticleFilterNode()
   {
       delete sys_model;
       delete meas_model;
       delete filter;
   }

   void CreateParticleFilter()
   {
       /****************************
        * NonLinear system model      *
        ***************************/

       // create gaussian
       ColumnVector sys_noise_Mu(STATE_SIZE);
       sys_noise_Mu(1) = MU_SYSTEM_NOISE_X;
       sys_noise_Mu(2) = MU_SYSTEM_NOISE_Y;
       sys_noise_Mu(3) = MU_SYSTEM_NOISE_THETA;

       SymmetricMatrix sys_noise_Cov(STATE_SIZE);
       sys_noise_Cov = 0.0;
       sys_noise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
       sys_noise_Cov(1,2) = 0.0;
       sys_noise_Cov(1,3) = 0.0;
       sys_noise_Cov(2,1) = 0.0;
       sys_noise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
       sys_noise_Cov(2,3) = 0.0;
       sys_noise_Cov(3,1) = 0.0;
       sys_noise_Cov(3,2) = 0.0;
       sys_noise_Cov(3,3) = SIGMA_SYSTEM_NOISE_THETA;

       Gaussian system_Uncertainty(sys_noise_Mu, sys_noise_Cov);

       // create the nonlinear system model
       sys_pdf = new NonlinearSystemPdf(system_Uncertainty);
       sys_model = new SystemModel<ColumnVector> (sys_pdf);


       /*********************************
        * NonLinear Measurement model   *
        ********************************/


       // Construct the measurement noise (a scalar in this case)
       ColumnVector meas_noise_Mu(MEAS_SIZE);
       meas_noise_Mu(1) = MU_MEAS_NOISE;
       meas_noise_Mu(2) = MU_MEAS_NOISE;
       meas_noise_Mu(3) = MU_MEAS_NOISE;
       SymmetricMatrix meas_noise_Cov(MEAS_SIZE);
       meas_noise_Cov(1,1) = SIGMA_MEAS_NOISE;
       meas_noise_Cov(1,2) = 0.0;
       meas_noise_Cov(1,3) = 0.0;
       meas_noise_Cov(2,1) = 0.0;
       meas_noise_Cov(2,2) = SIGMA_MEAS_NOISE;
       meas_noise_Cov(2,3) = 0.0;
       meas_noise_Cov(3,1) = 0.0;
       meas_noise_Cov(3,2) = 0.0;
       meas_noise_Cov(3,3) = SIGMA_MEAS_NOISE;

       Gaussian measurement_Uncertainty(meas_noise_Mu, meas_noise_Cov);


       meas_pdf = new NonlinearMeasurementPdf(measurement_Uncertainty, map_);
       meas_model = new MeasurementModel<ColumnVector,ColumnVector>(meas_pdf);

       /****************************
        * Linear prior DENSITY     *
        ***************************/
       // Continuous Gaussian prior (for Kalman filters)
       ColumnVector prior_Mu(STATE_SIZE);
       prior_Mu(1) = PRIOR_MU_X;
       prior_Mu(2) = PRIOR_MU_Y;
       prior_Mu(3) = PRIOR_MU_THETA;
       SymmetricMatrix prior_Cov(STATE_SIZE);
       prior_Cov(1,1) = PRIOR_COV_X;
       prior_Cov(1,2) = 0.0;
       prior_Cov(1,3) = 0.0;
       prior_Cov(2,1) = 0.0;
       prior_Cov(2,2) = PRIOR_COV_Y;
       prior_Cov(2,3) = 0.0;
       prior_Cov(3,1) = 0.0;
       prior_Cov(3,2) = 0.0;
       prior_Cov(3,3) = PRIOR_COV_THETA;
       Gaussian prior_cont(prior_Mu,prior_Cov);

       // Discrete prior for Particle filter (using the continuous Gaussian prior)
       vector<Sample<ColumnVector> > prior_samples(NUM_SAMPLES);
       prior_discr = new MCPdf<ColumnVector>(NUM_SAMPLES,STATE_SIZE);
       prior_cont.SampleFrom(prior_samples,NUM_SAMPLES,CHOLESKY,NULL);
       prior_discr->ListOfSamplesSet(prior_samples);

       /******************************
        * Construction of the Filter *
        ******************************/
       filter = new CustomParticleFilter (prior_discr, 0.5, NUM_SAMPLES/4.0);
   }

   // Need to create a Ranges message
   void MeasurementCb(geometry_msgs::Vector3 sensor)
   {
       ColumnVector measurement(3);
       measurement(1) = sensor.x / 100;
       measurement(2) = sensor.y / 100;
       measurement(3) = sensor.z / 100;
       //ROS_INFO("Measurement: %f",measurement(1));

       filter->Update(meas_model, measurement);
       PublishParticles();
       PublishPose();

   }

   // Need to create a velocities message type
   void InputCb(nav_msgs::Odometry msg)
   {
       if(!prevNavDataTime.isZero()) dt = (msg.header.stamp - prevNavDataTime).toSec();
       prevNavDataTime = msg.header.stamp;


       ColumnVector input(2);
       //Since msg comes from Odometry, need to go down the branches to get linear vel
       input(1) = msg.twist.twist.linear.x*dt*0.001;
       input(2) = msg.twist.twist.linear.y*dt*0.001;

       filter->Update(sys_model,input);
   }

   void PublishParticles()
   {
       geometry_msgs::PoseArray particles_msg;
       particles_msg.header.stamp = ros::Time::now();
       particles_msg.header.frame_id = "/map";

       vector<WeightedSample<ColumnVector> >::iterator sample_it;
       vector<WeightedSample<ColumnVector> > samples;

       samples = filter->getNewSamples();

       for(sample_it = samples.begin(); sample_it<samples.end(); sample_it++)
       {
           geometry_msgs::Pose pose;
           ColumnVector sample = (*sample_it).ValueGet();

           pose.position.x = sample(1);
           pose.position.y = sample(2);
           pose.orientation.z = sample(3);

           particles_msg.poses.insert(particles_msg.poses.begin(), pose);
       }
       particle_pub.publish(particles_msg);

   }

   void PublishPose()
   {
       Pdf<ColumnVector> * posterior = filter->PostGet();
       ColumnVector pose = posterior->ExpectedValueGet();
       SymmetricMatrix pose_cov = posterior->CovarianceGet();

       geometry_msgs::PoseStamped pose_msg;
       pose_msg.header.stamp = ros::Time::now();
       pose_msg.header.frame_id = "/map";

       pose_msg.pose.position.x = pose(1);
       pose_msg.pose.position.y = pose(2);
       pose_msg.pose.orientation.z = pose(3);

       pose_pub.publish(pose_msg);

       tf::Stamped<tf::Pose> map_to_base;

       /*tf_listener.lookupTransform ( "odom", "/base_link", ros::Time ( 0 ), odom_to_base_link );

       map_to_base_link.setOrigin( tf::Vector3(pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z) );

       map_to_base_link.setRotation (pose_msg.pose.orientation);
       */

       tf::Stamped<tf::Pose> odom_to_map;

       try
       {
          tf::Transform tmp_tf (tf::createQuaternionFromYaw(pose(3)),
                                tf::Vector3(pose(1),
                                pose(2),
                                0.0));

          tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(), pose_msg.header.stamp, "base_link");

          tf_->transformPose("odom", tmp_tf_stamped, odom_to_map);
       }
       catch(tf::TransformException)
       {
          ROS_DEBUG("Failed to subtract base to odom transform");
          return;
       }


       latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                                  tf::Point(odom_to_map.getOrigin()));

       transform_tolerance_.fromSec(0.1);

       ros::Time transform_expiration = (ros::Time::now() +
                                         transform_tolerance_);

       tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                            transform_expiration,
                                            "map", "odom");

       this->tfb_->sendTransform(tmp_tf_stamped);

       //br.sendTransform(tf::StampedTransform(map_to_base_link, ros::Time ( 0 ), "map", "base_link"));


       /*
       tf_listener.transformPose("map",
                                  pose_msg,
                                  map_to_base);

       latest_tf_ = tf::Transform(tf::Quaternion(map_to_base.getRotation()),
                                  tf::Point(map_to_base.getOrigin()));
       */

   }

   void requestMap()
   {
     // get map via RPC
     nav_msgs::GetMap::Request  req;
     nav_msgs::GetMap::Response resp;
     ROS_INFO("Requesting the map...");
     while(!ros::service::call("static_map", req, resp))
     {
       ROS_WARN("Request for map failed; trying again...");
       ros::Duration d(0.5);
       d.sleep();
     }
     handleMapMessage( resp.map );
   }

   void handleMapMessage(const nav_msgs::OccupancyGrid& msg)
   {
     ROS_INFO("Received a %d X %d map @ %.3f m/pix  Origin X %.3f Y %.3f\n",
              msg.info.width,
              msg.info.height,
              msg.info.resolution,
              msg.info.origin.position.x,
              msg.info.origin.position.y);

     freeMapDependentMemory();
     map_ = convertMap(msg);
     CreateParticleFilter();
   }

   void freeMapDependentMemory()
   {
     if( map_ != NULL ) {
       map_free( map_ );
       map_ = NULL;
     }

     if (sys_model)
       delete sys_model;
     if (meas_model)
       delete meas_model;
     if (filter)
       delete filter;
   }

   /**
    * Convert an OccupancyGrid map message into the internal
    * representation.  This allocates a map_t and returns it.
    */
   map_t* convertMap( const nav_msgs::OccupancyGrid& map_msg )
   {
     map_t* map = map_alloc();
     ROS_ASSERT(map);

     map->size_x = map_msg.info.width;
     map->size_y = map_msg.info.height;
     map->scale = map_msg.info.resolution;
     map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
     map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;

     ROS_INFO(" Size X=%d Y=%d Scale=%f Origin X=%f Y=%f", map->size_x, map->size_y, map->scale, map->origin_x, map->origin_y);
     // Convert to player format
     map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
     ROS_ASSERT(map->cells);

     for(int i=0;i<map->size_x * map->size_y;i++)
     {
       if(map_msg.data[i] == 0)
         map->cells[i].occ_state = -1;
       else if(map_msg.data[i] == 100)
         map->cells[i].occ_state = +1;
       else
         map->cells[i].occ_state = 0;
     }
     return map;
   }

};

int main(int argc, char** argv)
{
   ros::init(argc, argv, "ParticleFilterNode");
   ParticleFilterNode pfNode;
   ros::spin();
   return 0;
}

Originally posted by trixr4kdz on ROS Answers with karma: 45 on 2018-03-21

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

My guess is that you are using AMCL pkg for your particle filtered localization?

EDIT1 to answer your comments

it seems like a lot is happening just to get the map->odom transform

This is correct. According to AMCL wiki: "During operation amcl estimates the transformation of the base frame (~base_frame_id) in respect to the global frame (~global_frame_id) but it only publishes the transform between the global frame and the odometry frame (~odom_frame_id).". This means amcl will correct the pose of base_link by adjusting map->odom transform. I took a closer look at AMCL and figured that my previous answer was wrong. I edited it. So back to your original question, could you edit your question by specifying you robot's setup?

is the map->odom->base transform something we can just use the tf robot_state_publisher package for

Maybe you are talking about robot_pose_ekf ?

EDIT2 answer to your new comments

@trixr4kdz sorry for the late reply, I have been caught up with my project for the last couple days.

a way we can also get the map->odom without having to do the same thing amcl is doing

Certainly, you could, but if you ask whether there is an available package then I don't know. IMO, there are 2 options:

  1. consider open source your custom particle filer so everybody can have a look at what is going on. Without knowing what you are doing (for example: why your customer particle filter publishes map->base_link), it's really hard to help.

  2. re-implement the part of AMCL that publish map->odom (from #L1351 to #L1418). This shouldn't be too hard (I know it is NOT easy) since you must have the whole transform: map->odom->base_link->laser_scan from you particle filter.


Originally posted by tuandl with karma: 358 on 2018-03-22

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by trixr4kdz on 2018-03-22:
Sorry, I forgot to add that little detail. The particle filter we are using is based of this tutorial from the BFL package

Comment by trixr4kdz on 2018-03-22:
So just to confirm, you are saying that the problem isn't with our use of the robot_localization package but with the particle filter not being a map->odom transform, correct?

Comment by tuandl on 2018-03-22:
Correct. I am not familiar with BFL but a quick look at #L242 make me think...

Comment by tuandl on 2018-03-22:
...BFL works in a similar manner of AMCL.

Comment by trixr4kdz on 2018-03-22:
I see. Thank you very much for the help!

Comment by trixr4kdz on 2018-03-22:
I was looking at the amcl code on github and it seems like a lot is happening just to get the map->odom transform. For our particular case, is the map->odom->base transform something we can just use the tf robot_state_publisher package for?

Comment by trixr4kdz on 2018-03-23:
I edited my original post to add more details about our robot setup. Also apologies for the confusion, we are already using robot_localization to publish our odom->base transform, and I meant to ask if there's a way we can also get the map->odom without having to do the same thing amcl is doing.

Comment by trixr4kdz on 2018-03-26:
I added our particle filter code at the very bottom, but I also put it here, which might be better for you to see: https://repl.it/@trixr4kdz/ParticleFilterNodecpp. Lines 261-299 are our implementation of the AMCL code.

Comment by tuandl on 2018-03-27:
@trixr4kdz I will look at it and get back as soon as possible.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.