Kazunari Tanaka's profile - activity

The problem is incorrect settings of tf. I can see the evidence.

How to configure correct settings of global costmap plugins?(Spatio temporal voxel layer ) I'm trying to use Spatio temp

How to configure correct settings of global costmap plugins?(Spatio temporal voxel layer ) I'm trying to use Spatio temp

How to configure correct settings of global costmap plugins?(Spatio temporal voxel layer ) I'm trying to use Spatio temp

local_costmap:
local_costmap:
  ros__parameters:
    update_frequency: 5.0
    publish_frequency: 2.0
    global_frame: odom
    robot_base_frame: base_link
    use_sim_time: True
    rolling_window: true
    width: 3
    height: 3
    resolution: 0.05
    robot_radius: 0.3
    plugins: ["voxel_layer", "obstacle_layer", "inflation_layer"]
    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      cost_scaling_factor: 3.0
      inflation_radius: 0.55
    voxel_layer:
      plugin: "nav2_costmap_2d::VoxelLayer"
      enabled: True
      publish_voxel_map: True
      origin_z: 0.0
      z_resolution: 0.05
      z_voxels: 16
      max_obstacle_height: 2.0
      mark_threshold: 0
      observation_sources: scan
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: True
        marking: True
        data_type: "LaserScan"
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0
    obstacle_layer:
      plugin: "nav2_costmap_2d::ObstacleLayer"
      enabled: True
      observation_sources: bumper_front bumper_rear
      bumper_front:
        data_type: "PointCloud2"
        topic: /bumper_front_collision_cloud
        marking: True
        clearing: True
      bumper_rear:
        data_type: "PointCloud2"
        topic: /bumper_rear_collision_cloud
        marking: True
        clearing: True
    always_send_full_costmap: True
local_costmap_client:
  ros__parameters:
    use_sim_time: True
local_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: True
global_costmap:
global_costmap:
  ros__parameters:
    update_frequency: 1.0
    publish_frequency: 1.0
    global_frame: map
    robot_base_frame: base_link
    use_sim_time: True
    robot_radius: 0.22
    resolution: 0.05
    track_unknown_space: true
    plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
    obstacle_layer:
      plugin: "nav2_costmap_2d::ObstacleLayer"
      enabled: True
      observation_sources: scan
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: True
        marking: True
        data_type: "LaserScan"
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0
    static_layer:
      plugin: "nav2_costmap_2d::StaticLayer"
      map_subscribe_transient_local: True
    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      cost_scaling_factor: 3.0
      inflation_radius: 0.55
    always_send_full_costmap: True
global_costmap_client:
  ros__parameters:
    use_sim_time: True
global_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: True

I want to reflect bumper state to obstacle_layer. I tried to send a singleton pointcloud2 msg that has odom frame. But obstacle plugin says:

      Sensor origin at (0.0, 0.0) is out of map bounds. The costmap cannot raytrace for it.

So cost_maps did not reflect this message.

What is the proper way to set it up in the first place?

Thank you!

I want to write a BT node that recieve state of a bumper.

But my code can not catch messages of a bumper.

How can I write a BT Node that works properly with subscription?

Thank you!

#include <iostream>
#include <memory>
#include <string>

#include <rclcpp/qos.hpp>
#include <rclcpp/rclcpp.hpp>

#include "behaviortree_cpp_v3/condition_node.h"
#include "gazebo_msgs/msg/contacts_state.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

namespace nav2_behavior_tree {

class BumperCollisionCondition : public BT::ConditionNode {
  public:
    BumperCollisionCondition(const std::string &condition_name, const BT::NodeConfiguration &conf);

    BumperCollisionCondition() = delete;

    BT::NodeStatus tick() override;

    static BT::PortsList providedPorts() {
        return {};
    }

  private:
    rclcpp::Subscription<gazebo_msgs::msg::ContactsState>::SharedPtr contact_state_;
    rclcpp::Node::SharedPtr node_;
    rclcpp::Time lasttime_;
};

} // namespace nav2_behavior_tree

namespace nav2_behavior_tree {

BumperCollisionCondition::BumperCollisionCondition(
    const std::string &condition_name,
    const BT::NodeConfiguration &conf)
    : BT::ConditionNode(condition_name, conf) {
  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
  auto qos = rclcpp::SystemDefaultsQoS();
  qos.best_effort();
  contact_state_ = node_->create_subscription<gazebo_msgs::msg::ContactsState>(
    "bumper_front",
    qos,
    [this](const gazebo_msgs::msg::ContactsState::SharedPtr msg) -> void {
      RCLCPP_INFO(node_->get_logger(), "TEST"); // "TEST" never print.
      auto states = msg->states;
      lasttime_ = rclcpp::Time(msg->header.stamp);
    });
}

BT::NodeStatus BumperCollisionCondition::tick() {
    RCLCPP_INFO(node_->get_logger(), "test %d", lasttime_.nanoseconds()); // lasttime_ never update.
    return BT::NodeStatus::FAILURE;
}

} // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
    factory.registerNodeType<nav2_behavior_tree::BumperCollisionCondition>("BumperCollision");
}

How to configure correct settings of global costmap plugins?(Spatio temporal voxel layer ) I'm trying to use Spatio temp

I found the solution. The point is a pointcloud2 msg has to have a sensor frame. Furthermore I changed yaml params like

I am using slam_toolbox with arm64 architecture. But I encount SIGSEGV.

How do I fix this error?

backtrace:

  #0  tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::transformReadyCallback (this=0x555589d120, future=..., handle=2109) at /opt/ros/rolling/include/tf2_ros/message_filter.h:498
  #1  0x0000007fb721d0a8 in ?? () from /opt/ros/rolling/lib/libtf2_ros.so
  #2  0x0000007fb6f313f4 in tf2::BufferCore::testTransformableRequests() () from /opt/ros/rolling/lib/libtf2.so
  #3  0x0000007fb6f323d4 in tf2::BufferCore::setTransformImpl(tf2::Transform const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) ()
 from /opt/ros/rolling/lib/libtf2.so
 #4  0x0000007fb6f33354 in tf2::BufferCore::setTransform(geometry_msgs::msg::TransformStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) ()
 from /opt/ros/rolling/lib/libtf2.so
 #5  0x0000007fb7228df8 in tf2_ros::TransformListener::subscription_callback(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool) () from /opt/ros/rolling/lib/libtf2_ros.so
 #6  0x0000007fb72302c0 in std::_Function_handler<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >), std::_Bind<void (tf2_ros::TransformListener::*(tf2_ros::TransformListener*, std::_Placeholder<1>, bool))(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, bool)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >&&) () from /opt/ros/rolling/lib/libtf2_ros.so
 #7  0x0000007fb72414c0 in rclcpp::AnySubscriptionCallback<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&) () from /opt/ros/rolling/lib/libtf2_ros.so
 #8  0x0000007fb7241da4 in rclcpp::Subscription<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) ()
 from /opt/ros/rolling/lib/libtf2_ros.so
 #9  0x0000007fb79a76bc in ?? () from /opt/ros/rolling/lib/librclcpp.so
 #10 0x0000007fb79a82a8 in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) ()
 from /opt/ros/rolling/lib/librclcpp.so
 #11 0x0000007fb79a8ad4 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) ()
 from /opt/ros/rolling/lib/librclcpp.so
 #12 0x0000007fb79b1f34 in rclcpp::executors::SingleThreadedExecutor::spin() ()
 #13 0x0000007fb722843c in ?? () from /opt/ros/rolling/lib/libtf2_ros.so
 #14 0x0000007fb772dc2c in ?? () from /lib/aarch64-linux-gnu/libstdc++.so.6
 #15 0x0000007fb73d04fc in start_thread (arg=0x7fffffb98f) at pthread_create.c:477
 #16 0x0000007fb759d67c in thread_start () at ../sysdeps/unix/sysv/linux/aarch64/clone.S:78

I guess that these lines are incorrect. https://github.com/ros2/geometry2/blo... Because messages_ may be accessed before take the lock.

Am I correct?

Thank you!

How to reflect bumper collision to costmap? local_costmap: local_costmap: ros__parameters: update_frequency: 5.0

fix by https://github.com/ros2/geometry2/pull/386

Thank you for the suggestion. I try to make PR. https://github.com/ros2/geometry2/pull/386

Segmentation fault on slam_toolbox (tf2_ros) I am using slam_toolbox with arm64 architecture. But I encount SIGSEGV. Ho

Segmentation fault on slam_toolbox (tf2_ros) I am using slam_toolbox with arm64 architecture. But I encount SIGSEGV. Ho

Segmentation fault on slam_toolbox (tf2_ros) I am using slam_toolbox with arm64 architecture. But I encount SIGSEGV. Ho

I changed the bt tree to following. Then the callback function was called. I seem that the point is lifetime of BT node.

Thank you your response! My bt is here. <root main_tree_to_execute="MainTree"> <BehaviorTree ID="MainTree"

How to write BT node that subscribe a topic (navigation2). I want to write a BT node that recieve state of a bumper. Bu