diff --git a/src/mkmini_gazebo_steering_plugin/CMakeLists.txt b/src/mkmini_gazebo_steering_plugin/CMakeLists.txt new file mode 100644 index 0000000..0b69002 --- /dev/null +++ b/src/mkmini_gazebo_steering_plugin/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(mkmini_gazebo_steering_plugin) + +find_package(ament_cmake REQUIRED) +find_package(gazebo_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED src/steering_command_plugin.cpp) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +ament_target_dependencies(${PROJECT_NAME} gazebo_ros geometry_msgs rclcpp std_msgs) + +install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib) +ament_package() diff --git a/src/mkmini_gazebo_steering_plugin/package.xml b/src/mkmini_gazebo_steering_plugin/package.xml new file mode 100644 index 0000000..a9adcb0 --- /dev/null +++ b/src/mkmini_gazebo_steering_plugin/package.xml @@ -0,0 +1,13 @@ + + + mkmini_gazebo_steering_plugin + 1.0.0 + Independent visual steering for the Gazebo MK-mini front wheels. + Autonomy Studio + Apache-2.0 + ament_cmake + gazebo_ros + geometry_msgs + rclcpp + std_msgs + diff --git a/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp b/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp new file mode 100644 index 0000000..802ffd2 --- /dev/null +++ b/src/mkmini_gazebo_steering_plugin/src/steering_command_plugin.cpp @@ -0,0 +1,86 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace mkmini_gazebo_steering_plugin +{ +class SteeringCommandPlugin : public gazebo::ModelPlugin +{ +public: + void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override + { + left_joint_ = model->GetJoint(sdf->Get("left_joint")); + right_joint_ = model->GetJoint(sdf->Get("right_joint")); + if (!left_joint_ || !right_joint_) { + gzerr << "MK-mini visual steering joints were not found.\n"; + return; + } + wheelbase_ = sdf->Get("wheelbase", 0.60).first; + track_width_ = sdf->Get("track_width", 0.72).first; + node_ = gazebo_ros::Node::Get(sdf); + const auto topic = sdf->Get("topic", "/mkmini/steering_command").first; + subscription_ = node_->create_subscription( + topic, rclcpp::QoS(1), + [this](const std_msgs::msg::Float64::SharedPtr message) { + std::lock_guard lock(mutex_); + target_angle_ = std::clamp(message->data, -0.5, 0.5); + has_target_ = true; + }); + cmd_vel_subscription_ = node_->create_subscription( + "/cmd_vel", rclcpp::QoS(10), + [this](const geometry_msgs::msg::Twist::SharedPtr message) { + std::lock_guard lock(mutex_); + target_angle_ = std::clamp(message->angular.z, -0.5, 0.5); + has_target_ = true; + }); + update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( + std::bind(&SteeringCommandPlugin::OnUpdate, this)); + } + +private: + void OnUpdate() + { + std::lock_guard lock(mutex_); + if (!has_target_ || !left_joint_ || !right_joint_) return; + const auto [left_angle, right_angle] = AckermannAngles(target_angle_); + left_joint_->SetPosition(0, left_angle); + right_joint_->SetPosition(0, right_angle); + } + + std::pair AckermannAngles(double steering_angle) const + { + if (std::abs(steering_angle) < 1e-6) return {0.0, 0.0}; + const auto direction = steering_angle > 0.0 ? 1.0 : -1.0; + const auto radius = wheelbase_ / std::tan(std::abs(steering_angle)); + const auto inner = std::atan(wheelbase_ / std::max(0.01, radius - track_width_ * 0.5)); + const auto outer = std::atan(wheelbase_ / (radius + track_width_ * 0.5)); + return direction > 0.0 ? std::make_pair(inner, outer) : std::make_pair(-outer, -inner); + } + + gazebo::physics::JointPtr left_joint_; + gazebo::physics::JointPtr right_joint_; + gazebo::event::ConnectionPtr update_connection_; + gazebo_ros::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr cmd_vel_subscription_; + std::mutex mutex_; + double wheelbase_{0.60}; + double track_width_{0.72}; + double target_angle_{0.0}; + bool has_target_{false}; +}; + +GZ_REGISTER_MODEL_PLUGIN(SteeringCommandPlugin) +} // namespace mkmini_gazebo_steering_plugin diff --git a/src/nav2_turtlebot_simulator/CMakeLists.txt b/src/nav2_turtlebot_simulator/CMakeLists.txt index a11cfc3..30b70f5 100644 --- a/src/nav2_turtlebot_simulator/CMakeLists.txt +++ b/src/nav2_turtlebot_simulator/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.8) project(nav2_turtlebot_simulator) find_package(ament_cmake REQUIRED) +find_package(mkmini_gazebo_steering_plugin REQUIRED) install(DIRECTORY launch worlds models DESTINATION share/${PROJECT_NAME}) diff --git a/src/nav2_turtlebot_simulator/launch/simulator.launch.py b/src/nav2_turtlebot_simulator/launch/simulator.launch.py index 324ecd3..e74be44 100644 --- a/src/nav2_turtlebot_simulator/launch/simulator.launch.py +++ b/src/nav2_turtlebot_simulator/launch/simulator.launch.py @@ -220,21 +220,33 @@ def create_mkmini_world(gazebo_dir): except ValueError: continue - # The MKMini lidar is centered at 0.52 m. Raise the original 0.5 m white - # pillars so they remain observable obstacles instead of falling below the - # scan plane. - for element in (*model.findall('.//collision'), *model.findall('.//visual')): - cylinder = element.find('./geometry/cylinder') - if cylinder is None: - continue - if cylinder.findtext('radius') != '0.15' or cylinder.findtext('length') != '0.5': - continue - cylinder.find('length').text = '1.2' - pose = element.find('pose') - if pose is None: - continue - values = pose.text.split() - if len(values) == 6: + # Keep only the centre post from TurtleBot3 World's 3x3 white-pillar grid. + # This is applied to the generated MKMini world only; Burger keeps the + # original obstacle layout. Remove both visual and collision shapes so the + # Gazebo scene and Nav2 costmap describe the same space. + for link in model.findall('.//link'): + for element in list(link): + if element.tag not in {'collision', 'visual'}: + continue + cylinder = element.find('./geometry/cylinder') + if cylinder is None: + continue + if cylinder.findtext('radius') != '0.15' or cylinder.findtext('length') != '0.5': + continue + pose = element.find('pose') + values = pose.text.split() if pose is not None and pose.text else [] + if len(values) != 6: + continue + try: + x, y = float(values[0]), float(values[1]) + except ValueError: + continue + if abs(x) > 1e-6 or abs(y) > 1e-6: + link.remove(element) + continue + # The MKMini lidar is centered at 0.52 m. Raise the remaining + # centre post so it stays observable instead of below the scan. + cylinder.find('length').text = '1.2' values[2] = '0.6' pose.text = ' '.join(values) diff --git a/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf index 63e3325..129d046 100644 --- a/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf +++ b/src/nav2_turtlebot_simulator/models/mkmini_gazebo/model.sdf @@ -1,8 +1,8 @@ - + 0 0 0.16 0 0 0 @@ -13,37 +13,25 @@ 0.56 0 0.68 0 0 00.06 0.42 0.100.95 0.65 0.08 10.95 0.65 0.08 1 - 0.30 0 0 0 0 01.00.010.010.01 - - 0 0.36 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1 - 0 -0.36 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1 - + 0.30 0 0 0 0 01.00.010.010.01 base_linkfront_fork0 0 1-0.50.5 - 0.30 0 0 0 0 01.20.020.010.020 0 0 1.5708 0 00.140.18 front_forkfront_drive_wheel0 1 0 + 0.30 0.36 0 0 0 00.050.00010.00010.00010 0 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1 + base_linkfront_left_visual_wheel0 0 1-0.60.6 + 0.30 -0.36 0 0 0 00.050.00010.00010.00010 0 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1 + base_linkfront_right_visual_wheel0 0 1-0.60.6 + -0.30 0.36 0 0 0 01.00.020.010.020 0 0 1.5708 0 00.140.100 0 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1 base_linkrear_left_wheel0 1 0 - -0.30 -0.36 0 0 0 01.00.020.010.020 0 0 1.5708 0 00.140.100 0 0 1.5708 0 00.140.100.04 0.04 0.04 10.04 0.04 0.04 1 base_linkrear_right_wheel0 1 0 - 0 0 0.9 0 0 00.20.0010.0010.001 - 0.0750.100.85 0.12 0.10 10.85 0.12 0.10 1 - true10true3601-3.141593.1415911000.2018.00.02~/out:=/raw/lidarsensor_msgs/PointCloud2os_lidar - + 0 0 0.9 0 0 00.20.0010.0010.0010.0750.100.85 0.12 0.10 10.85 0.12 0.10 1true10true3601-3.141593.1415911000.2018.00.02~/out:=/raw/lidarsensor_msgs/PointCloud2os_lidar base_linkos_lidar - - cmd_vel:=/cmd_velodom:=/odom - front_steering_jointfront_drive_joint - rear_left_jointrear_right_joint - 0.720.280.28 - 120.01.50.8 - truetruetrue - odombase_link - + cmd_vel:=/cmd_velodom:=/odomfront_steering_jointfront_drive_jointrear_left_jointrear_right_joint0.720.280.28120.01.50.0truetruetrueodombase_link + /mkmini/steering_commandfront_left_visual_steering_jointfront_right_visual_steering_joint0.600.72 diff --git a/src/nav2_turtlebot_simulator/package.xml b/src/nav2_turtlebot_simulator/package.xml index 47de6a7..dad7608 100644 --- a/src/nav2_turtlebot_simulator/package.xml +++ b/src/nav2_turtlebot_simulator/package.xml @@ -6,6 +6,7 @@ Autonomy Studio Apache-2.0 ament_cmake + mkmini_gazebo_steering_plugin gazebo_ros launch launch_ros