Added fake bumper in sim

Signed-off-by: Juancams <[email protected]>

CMakeLists.txt

@@ -6,15 +6,52 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(kobuki_ros_interfaces REQUIRED)
+
+set(dependencies
+  rclcpp
+  sensor_msgs
+  geometry_msgs
+  kobuki_ros_interfaces
+)
+
+include_directories(include)
+
+add_library(${PROJECT_NAME} SHARED
+  src/kobuki/FakeBumper.cpp
+)
+ament_target_dependencies(${PROJECT_NAME} ${dependencies})
+
+add_executable(fake_bumer_node src/fake_bumper_node.cpp)
+ament_target_dependencies(fake_bumer_node ${dependencies})
+target_link_libraries(fake_bumer_node ${PROJECT_NAME})
+
+install(TARGETS
+  fake_bumer_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  DESTINATION lib/${PROJECT_NAME}
+)
 
 install(DIRECTORY launch config maps
         DESTINATION share/${PROJECT_NAME})
 
+install(
+  DIRECTORY include/
+  DESTINATION include/
+)       
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+ament_export_include_directories(include)
+ament_export_dependencies(${dependencies})
+
 ament_package()

include/kobuki/FakeBumper.hpp

@@ -0,0 +1,50 @@
+// Copyright 2024 Juan Carlos Manzanares Serrano
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef KOBUKI__FAKE_BUMPER_HPP_
+#define KOBUKI__FAKE_BUMPER_HPP_
+
+#include <utility>
+
+#include "rclcpp/rclcpp.hpp"
+#include "kobuki_ros_interfaces/msg/bumper_event.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+
+namespace kobuki
+{
+
+using std::placeholders::_1;
+
+class FakeBumper : public rclcpp::Node
+{
+public:
+  FakeBumper();
+
+private:
+  using LaserScan = sensor_msgs::msg::LaserScan;
+  using BumperEvent = kobuki_ros_interfaces::msg::BumperEvent;
+
+  void laserCallback(const LaserScan::UniquePtr msg);
+
+  rclcpp::Publisher<BumperEvent>::SharedPtr bumper_pub_;
+  rclcpp::Subscription<LaserScan>::SharedPtr scan_sub_;
+
+  BumperEvent pressed_;
+
+  static constexpr float OBSTACLE_DISTANCE = 0.2f;
+};
+
+}  // namespace kobuki
+
+#endif  // KOBUKI__FAKE_BUMPER_HPP_

launch/simulation.launch.py

@@ -161,6 +161,8 @@ def generate_launch_description():
                                             '0.0', '0.0', '0.0',
                                             'base_link',
                                             'base_footprint'])
+
+    fake_bumper_cmd = Node(package='kobuki', executable='fake_bumer_node', output='screen')
 
     ld = LaunchDescription()
     ld.add_action(declare_world_cmd)
@@ -176,6 +178,7 @@ def generate_launch_description():
     ld.add_action(tf_footprint2base_cmd)
     ld.add_action(robot_entity_cmd)
     ld.add_action(world_entity_cmd)
+    ld.add_action(fake_bumper_cmd)
 
     packages = ['kobuki_description']
     model_path = get_model_paths(packages)

src/fake_bumper_node.cpp

@@ -0,0 +1,28 @@
+// Copyright 2024 Juan Carlos Manzanares Serrano
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "kobuki/FakeBumper.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  auto fake_bumper_node = std::make_shared<kobuki::FakeBumper>();
+  rclcpp::spin(fake_bumper_node);
+
+  rclcpp::shutdown();
+
+  return 0;
+}

src/kobuki/FakeBumper.cpp

@@ -0,0 +1,72 @@
+// Copyright 2024 Juan Carlos Manzanares Serrano
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "kobuki/FakeBumper.hpp"
+
+namespace kobuki
+{
+FakeBumper::FakeBumper()
+: Node("fake_bumper")
+{
+  pressed_.state = BumperEvent::RELEASED;
+
+  bumper_pub_ =
+    this->create_publisher<BumperEvent>("/events/bumper", 10);
+  scan_sub_ = create_subscription<LaserScan>(
+    "/scan", rclcpp::SensorDataQoS(),
+    std::bind(&FakeBumper::laserCallback, this, _1));
+}
+
+void
+FakeBumper::laserCallback(const LaserScan::UniquePtr msg)
+{
+  if (pressed_.state) {
+
+    for (size_t i = 0; i < msg->ranges.size(); i++) {
+      if (msg->ranges[i] < OBSTACLE_DISTANCE) {
+        return;
+      }
+    }
+
+    pressed_.state = BumperEvent::RELEASED;
+    bumper_pub_->publish(pressed_);
+  } else {
+    size_t pos = 0, threshold_left = msg->ranges.size() / 4,
+      threshold_right = msg->ranges.size() - threshold_left;
+
+    for (size_t i = 0; i < msg->ranges.size(); i++) {
+      if (msg->ranges[i] < OBSTACLE_DISTANCE && (i<threshold_left || i> threshold_right)) {
+        pressed_.state = BumperEvent::PRESSED;
+        pos = i;
+        break;
+      }
+    }
+
+    if (!pressed_.state) {return;}
+
+    size_t threshold_center_left = threshold_left / 3,
+      threshold_center_right = msg->ranges.size() - threshold_center_left;
+
+    if (pos > threshold_center_left && pos < threshold_left) {
+      pressed_.bumper = BumperEvent::LEFT;
+    } else if (pos > threshold_right && pos < threshold_center_right) {
+      pressed_.bumper = BumperEvent::RIGHT;
+    } else {
+      pressed_.bumper = BumperEvent::CENTER;
+    }
+
+    bumper_pub_->publish(pressed_);
+  }
+}
+}  // namespace kobuki