Align MKMini Gazebo steering and world scale
This commit is contained in:
@@ -44,7 +44,18 @@ public:
|
|||||||
"/cmd_vel", rclcpp::QoS(10),
|
"/cmd_vel", rclcpp::QoS(10),
|
||||||
[this](const geometry_msgs::msg::Twist::SharedPtr message) {
|
[this](const geometry_msgs::msg::Twist::SharedPtr message) {
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
target_angle_ = std::clamp(message->angular.z, -0.5, 0.5);
|
// Twist.angular.z is yaw rate, not steering angle. Convert it using
|
||||||
|
// the signed vehicle speed so reverse Reeds-Shepp segments keep the
|
||||||
|
// same physical curvature as their planned path.
|
||||||
|
if (std::abs(message->linear.x) >= 0.01) {
|
||||||
|
target_angle_ = std::clamp(
|
||||||
|
std::atan(wheelbase_ * message->angular.z / message->linear.x),
|
||||||
|
-0.5,
|
||||||
|
0.5);
|
||||||
|
} else {
|
||||||
|
// Preserve the direct angle fallback used by manual J/L steering.
|
||||||
|
target_angle_ = std::clamp(message->angular.z, -0.5, 0.5);
|
||||||
|
}
|
||||||
has_target_ = true;
|
has_target_ = true;
|
||||||
});
|
});
|
||||||
update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
|
update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin(
|
||||||
|
|||||||
@@ -33,7 +33,7 @@ LOCAL_SUN_LIGHT = ''' <light type="directional" name="sun">
|
|||||||
<cast_shadows>false</cast_shadows><pose>0 0 10 0 0 0</pose><diffuse>0.8 0.8 0.8 1</diffuse><specular>0.2 0.2 0.2 1</specular><direction>-0.5 0.1 -0.9</direction>
|
<cast_shadows>false</cast_shadows><pose>0 0 10 0 0 0</pose><diffuse>0.8 0.8 0.8 1</diffuse><specular>0.2 0.2 0.2 1</specular><direction>-0.5 0.1 -0.9</direction>
|
||||||
</light>'''
|
</light>'''
|
||||||
|
|
||||||
MKMINI_WORLD_SPACING_SCALE = 1.6
|
MKMINI_WORLD_SPACING_SCALE = 1.0
|
||||||
|
|
||||||
|
|
||||||
def replace_house_visual_meshes_with_boxes(house_model):
|
def replace_house_visual_meshes_with_boxes(house_model):
|
||||||
@@ -177,7 +177,7 @@ def create_flat_house_world(gazebo_dir):
|
|||||||
|
|
||||||
|
|
||||||
def create_mkmini_world(gazebo_dir):
|
def create_mkmini_world(gazebo_dir):
|
||||||
"""Expand the TurtleBot3 obstacle layout without changing obstacle sizes."""
|
"""Create the MKMini world variant without changing map-scale coordinates."""
|
||||||
source_model_path = os.path.join(gazebo_dir, 'models', 'turtlebot3_world', 'model.sdf')
|
source_model_path = os.path.join(gazebo_dir, 'models', 'turtlebot3_world', 'model.sdf')
|
||||||
models_dir = os.path.join(tempfile.gettempdir(), 'nav2_mkmini_models')
|
models_dir = os.path.join(tempfile.gettempdir(), 'nav2_mkmini_models')
|
||||||
model_dir = os.path.join(models_dir, 'turtlebot3_world_mkmini')
|
model_dir = os.path.join(models_dir, 'turtlebot3_world_mkmini')
|
||||||
@@ -190,8 +190,8 @@ def create_mkmini_world(gazebo_dir):
|
|||||||
raise RuntimeError('turtlebot3_world model is missing from model.sdf')
|
raise RuntimeError('turtlebot3_world model is missing from model.sdf')
|
||||||
model.attrib['name'] = 'turtlebot3_world_mkmini'
|
model.attrib['name'] = 'turtlebot3_world_mkmini'
|
||||||
|
|
||||||
# Only object positions change. Keeping their geometry unchanged widens the
|
# Keep object geometry and coordinates aligned with the base world. MKMini
|
||||||
# driving lanes for the larger MKMini collision footprint.
|
# clearance comes from its footprint and inflation configuration.
|
||||||
for pose in model.findall('.//pose'):
|
for pose in model.findall('.//pose'):
|
||||||
values = pose.text.split()
|
values = pose.text.split()
|
||||||
if len(values) != 6:
|
if len(values) != 6:
|
||||||
|
|||||||
Reference in New Issue
Block a user