morten's profile - activity

I'm using the ekf in robot_localization, for some reason it doesn't seem to want to go above the odd 10Hz. I've checked my imu and odometry rates, and they're both 50Hz as I've set them.

I launch the ekf node by ros2 launch tractor_gazebo ekf.launch.py use_sim_time:=true and I check the individual frequencies:

$ ros2 param get /ekf_localization_odom frequency 
Double value is: 30.0
$ ros2 topic hz /imu/data 
average rate: 49.887
    min: 0.020s max: 0.021s std dev: 0.00020s window: 51
$ ros2 topic hz /odometry/wheel 
average rate: 49.910
    min: 0.020s max: 0.021s std dev: 0.00018s window: 51
average rate: 49.853
    min: 0.020s max: 0.021s std dev: 0.00020s window: 101
$ ros2 topic hz /odometry/filtered/odom 
average rate: 9.956
    min: 0.100s max: 0.101s std dev: 0.00033s window: 11
average rate: 9.961
    min: 0.099s max: 0.101s std dev: 0.00043s window: 21

with the launch and config files added at the bottom.

Note: I have tried setting the ekf frequency to less than 10Hz, and this is can follow fine. So, e.g. changing the parameter in the yaml to 5Hz results in publishing to /odometry/filtered/odom at 5Hz, setting it to 15Hz instead results in the topic publishing at 10Hz. This is the core of the problem

ekf.launch.py:

import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node

def generate_launch_description():
    # get directories
    tractor_gazebo_dir = get_package_share_directory("tractor_gazebo")

    # Create the launch configuration variables
    use_sim_time = LaunchConfiguration("use_sim_time")

    # declare launch arguments
    declare_use_sim_time = DeclareLaunchArgument("use_sim_time")

    # Specify the actions
    robot_localization_odom = Node(
        package="robot_localization",
        executable="ekf_node",
        name="ekf_localization_odom",
        output="screen",
        parameters=[
            {"use_sim_time": use_sim_time},
            os.path.join(
                tractor_gazebo_dir,
                "config",
                "dual_ekf_navsat_toolcarrier.yaml",
            ),
        ],
        remappings=[("odometry/filtered", "odometry/filtered/odom")],
    )

    return LaunchDescription(
        [
            declare_use_sim_time,
            robot_localization_odom,
        ]
    )

config file:

ekf_localization_odom:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.0 #0.1
    two_d_mode: true
    transform_time_offset: 0.0 #0.01
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: odometry/wheel
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  true,  false,
                   false, false, true,
                   false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false

    imu0: imu/data
    imu0_config: [false, false, false,
                  true,  true,  true,
                  false, false, false,
                  true,  true,  true,
                  true,  true,  true]
    imu0_nodelay: false
    imu0_differential: false
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true

    use_control: false

Edit: Following @osilva's comments I have checked the sim time parameter of all the nodes:

morten@agrirobot:~$ ros2 param list
/ackermann_drive:
  use_sim_time
/camera_controller_front:
  use_sim_time
/camera_controller_middle:
  use_sim_time
/controller_manager:
  use_sim_time
/ekf_localization_odom:
  use_sim_time
/gazebo:
  use_sim_time
/gazebo_ros2_control:
  use_sim_time
/velodyne_front/gazebo_ros_laser_controller:
  use_sim_time
/gps:
  use_sim_time
/imu:
  use_sim_time
/joint_state_publisher:
  use_sim_time
/robot_state_publisher:
  use_sim_time

Note, I've trimmed the other parameters so as not to go on too long, I have checked each and every one of these:

morten@agrirobot:~$ ros2 param get /ackermann_drive use_sim_time 
Boolean value is: True
morten@agrirobot:~$ ros2 param get /camera_controller_front use_sim_time 
Boolean value is: True ...

I am trying to write a pointcloud filtering node in ROS2 foxy using pcl but I am running into some problems I think are resulting from the publishing.

The resulting error is

~/ros2_ws$ colcon build --symlink-install --packages-select pointcloud_filters
Starting >>> pointcloud_filters
--- stderr: pointcloud_filters                                
** WARNING ** io features related to pcap will be disabled
** WARNING ** io features related to png will be disabled
** WARNING ** io features related to libusb-1.0 will be disabled
/usr/bin/ld: CMakeFiles/pointcloud_filters_node.dir/src/pointcloud_filters_node.cpp.o: in function `rclcpp::Publisher<pcl::PointCloud<pcl::PointXYZI>, std::allocator<void> >::Publisher(rclcpp::node_interfaces::NodeBaseInterface*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rclcpp::QoS const&, rclcpp::PublisherOptionsWithAllocator<std::allocator<void> > const&)':
pointcloud_filters_node.cpp:(.text._ZN6rclcpp9PublisherIN3pcl10PointCloudINS1_9PointXYZIEEESaIvEEC2EPNS_15node_interfaces17NodeBaseInterfaceERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEERKNS_3QoSERKNS_29PublisherOptionsWithAllocatorIS5_EE[_ZN6rclcpp9PublisherIN3pcl10PointCloudINS1_9PointXYZIEEESaIvEEC5EPNS_15node_interfaces17NodeBaseInterfaceERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEERKNS_3QoSERKNS_29PublisherOptionsWithAllocatorIS5_EE]+0x6e): undefined reference to `rosidl_message_type_support_t const* rosidl_typesupport_cpp::get_message_type_support_handle<pcl::PointCloud<pcl::PointXYZI> >()'
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/pointcloud_filters_node.dir/build.make:317: pointcloud_filters_node] Error 1
make[1]: *** [CMakeFiles/Makefile2:78: CMakeFiles/pointcloud_filters_node.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< pointcloud_filters [12.3s, exited with code 2]

Summary: 0 packages finished [12.4s]
  1 package failed: pointcloud_filters
  1 package had stderr output: pointcloud_filters

so its complaining about some undefined reference. I'm not entirely sure what the problem is but it seems like it could be similar to https://answers.ros.org/question/3117.... I looked at the interfaces for sensor_msgs/msg/PointCloud2 and sensor_msgs/msg./PointField and it looks identical to pcl::PointCloud so maybe I should manually transfer the attributes?

CPP file:

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

#include "pcl_conversions/pcl_conversions.h"

#include "pcl/filters/radius_outlier_removal.h"
#include "pcl/impl/point_types.hpp"
#include "pcl/point_cloud.h"

class PCFilter : public rclcpp::Node {
public:
  PCFilter() : Node("pointcloud_filters") {
    pub_ = this->create_publisher<pcl::PointCloud<pcl::PointXYZI>>(
        "filters_output", 1);
    sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        "filters_input", 1,
        std::bind(&PCFilter::cloud_callback, this, std::placeholders::_1));

    this->declare_parameter("pointcloud_filters/neighbors", 1);
    this->declare_parameter("pointcloud_filters/radius", 0.2);
    this->declare_parameter("pointcloud_filters/distance", 5.0);
    this->declare_parameter("pointcloud_filters/intensity_threshold", 8);

    neighbors_ = this->get_parameter("pointcloud_filters/neighbors").as_int();
    radius_ = this->get_parameter("pointcloud_filters/radius").as_double();
    distance_ = this->get_parameter("pointcloud_filters/distance").as_double();
    intensity_threshold_ =
        this->get_parameter("pointcloud_filters/intensity_threshold").as_int();

    RCLCPP_INFO(this->get_logger(),
                "Filter "
                "pararms:\n\tneighbors:%d\t\nradius:%f\n\tdistance:%"
                "f\n\tintensity_threshold:%d",
                neighbors_, radius_, distance_, intensity_threshold_);
  }

private:
  void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
        new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(
        new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_dust_filtered(
        new pcl::PointCloud<pcl::PointXYZI>);

    pcl::fromROSMsg(*msg, *cloud);

    if (cloud->size() > 0) {
      // Filter dust based on low intensity within a certain distance from the
      // lidar
      double dist = 0.0;
      for (std::size_t n = 0; n < cloud->size(); n++) {
        dist = std::sqrt(std::pow(cloud->points[n].x, 2) +
                         std::pow(cloud->points[n].y, 2) +
                         std::pow(cloud->points[n].z, 2));
        if (dist < distance_) {
          if (cloud->points[n].intensity > intensity_threshold_) {
            cloud_dust_filtered->push_back(cloud->points[n]);
          }
        } else {
          cloud_dust_filtered->push_back(cloud->points[n]);
        }
      }

      cloud_dust_filtered->header.frame_id = cloud->header.frame_id;
      cloud_dust_filtered->header.stamp = cloud->header.stamp;

      // Create radius outlier removal filter
      pcl::RadiusOutlierRemoval<pcl::PointXYZI> outrem;
      outrem.setInputCloud(cloud_dust_filtered);
      outrem.setRadiusSearch(radius_);
      outrem.setMinNeighborsInRadius(neighbors_);
      // Apply filter
      outrem.filter ...