diff --git a/CMakeLists.txt b/CMakeLists.txt index cbe4a0d..83db736 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,10 +6,44 @@ 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() @@ -17,4 +51,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() +ament_export_include_directories(include) +ament_export_dependencies(${dependencies}) + ament_package() diff --git a/include/kobuki/FakeBumper.hpp b/include/kobuki/FakeBumper.hpp new file mode 100644 index 0000000..7ef6a07 --- /dev/null +++ b/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 + +#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::SharedPtr bumper_pub_; + rclcpp::Subscription::SharedPtr scan_sub_; + + BumperEvent pressed_; + + static constexpr float OBSTACLE_DISTANCE = 0.2f; +}; + +} // namespace kobuki + +#endif // KOBUKI__FAKE_BUMPER_HPP_ diff --git a/launch/simulation.launch.py b/launch/simulation.launch.py index 934c3ca..099039f 100644 --- a/launch/simulation.launch.py +++ b/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) diff --git a/src/fake_bumper_node.cpp b/src/fake_bumper_node.cpp new file mode 100644 index 0000000..0cb4314 --- /dev/null +++ b/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(); + rclcpp::spin(fake_bumper_node); + + rclcpp::shutdown(); + + return 0; +} diff --git a/src/kobuki/FakeBumper.cpp b/src/kobuki/FakeBumper.cpp new file mode 100644 index 0000000..e5dd689 --- /dev/null +++ b/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("/events/bumper", 10); + scan_sub_ = create_subscription( + "/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_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