diff --git a/rmf_building_sim_gz_plugins/src/door.cpp b/rmf_building_sim_gz_plugins/src/door.cpp index ae48b75..3df9d87 100644 --- a/rmf_building_sim_gz_plugins/src/door.cpp +++ b/rmf_building_sim_gz_plugins/src/door.cpp @@ -136,7 +136,8 @@ class DoorPlugin auto target_vel = calculate_target_velocity(target_pos, cur_pos, _last_cmd_vel[joint_entity], dt, door.params); - ecm.CreateComponent(joint_entity, components::JointPositionReset( + ecm.CreateComponent(joint_entity, + components::JointPositionReset( {cur_pos + target_vel * dt})); _last_cmd_vel[joint_entity] = target_vel; } @@ -199,10 +200,12 @@ class DoorPlugin { if (door->Data().ros_interface == false) { - gzmsg << "Ignoring door " << msg->door_name << " because it doesn't have a ros interface" << std::endl; + gzmsg << "Ignoring door " << msg->door_name << + " because it doesn't have a ros interface" << std::endl; return; } - auto door_command = msg->requested_mode.value == msg->requested_mode.MODE_OPEN ? + auto door_command = msg->requested_mode.value == + msg->requested_mode.MODE_OPEN ? DoorModeCmp::OPEN : DoorModeCmp::CLOSE; ecm->CreateComponent(entity, components::DoorCmd(door_command)); diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp index 44626c5..0822ca1 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_plugins/src/lift.cpp @@ -431,13 +431,15 @@ class LiftPlugin if (available_floors.find(msg->destination_floor) == available_floors.end()) { - gzwarn << "Received request for unavailable floor [" << msg->destination_floor << "]" << std::endl; + gzwarn << "Received request for unavailable floor [" << + msg->destination_floor << "]" << std::endl; return; } LiftCommand lift_command; lift_command.request_type = msg->request_type; lift_command.destination_floor = msg->destination_floor; - lift_command.session_id = msg->request_type == msg->REQUEST_END_SESSION ? + lift_command.session_id = msg->request_type == + msg->REQUEST_END_SESSION ? "" : msg->session_id; lift_command.door_state = msg->door_state == msg->DOOR_OPEN ? @@ -453,7 +455,8 @@ class LiftPlugin cur_lift_cmd.request_type != msg->request_type || cur_lift_cmd.session_id != msg->session_id) { - gzwarn << "Discarding request: [" << msg->lift_name <<"] is busy at the moment" << std::endl; + gzwarn << "Discarding request: [" << msg->lift_name << + "] is busy at the moment" << std::endl; return; } } diff --git a/rmf_building_sim_gz_plugins/src/toggle_floors/toggle_floors.cpp b/rmf_building_sim_gz_plugins/src/toggle_floors/toggle_floors.cpp index cda9078..b0070a9 100644 --- a/rmf_building_sim_gz_plugins/src/toggle_floors/toggle_floors.cpp +++ b/rmf_building_sim_gz_plugins/src/toggle_floors/toggle_floors.cpp @@ -102,7 +102,8 @@ void toggle_floors::get_plugin_config() const std::string WORLD_NAME = "sim_world"; gz::msgs::GUI res; - std::string service = gz::transport::TopicUtils::AsValidTopic("/world/" + WORLD_NAME + + std::string service = gz::transport::TopicUtils::AsValidTopic("/world/" + + WORLD_NAME + "/gui/info"); node.Request(service, timeout, res, result); @@ -126,18 +127,18 @@ void toggle_floors::LoadConfig(const tinyxml2::XMLElement* _pluginElem) this->title = "Toggle Floors"; const tinyxml2::XMLElement* plugin_config = [&] + { + if (_pluginElem->FirstChildElement("floor")) { - if (_pluginElem->FirstChildElement("floor")) - { - return _pluginElem->FirstChildElement("floor"); - } - else - { - get_plugin_config(); - const tinyxml2::XMLElement* p = _config_xml.RootElement(); - return p; - } - } (); + return _pluginElem->FirstChildElement("floor"); + } + else + { + get_plugin_config(); + const tinyxml2::XMLElement* p = _config_xml.RootElement(); + return p; + } + } (); if (plugin_config) { diff --git a/rmf_robot_sim_common/src/slotcar_common.cpp b/rmf_robot_sim_common/src/slotcar_common.cpp index bb4ab88..301da85 100644 --- a/rmf_robot_sim_common/src/slotcar_common.cpp +++ b/rmf_robot_sim_common/src/slotcar_common.cpp @@ -624,12 +624,12 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive( pause_request.type == pause_request.TYPE_PAUSE_IMMEDIATELY; const bool e_stop = [&]() -> bool - { - if (result.target_linear_speed_now > 0.0) - return emergency_stop(obstacle_positions, current_heading); + { + if (result.target_linear_speed_now > 0.0) + return emergency_stop(obstacle_positions, current_heading); - return false; - } (); + return false; + } (); const bool stop = immediate_pause || e_stop; if (immediate_pause) @@ -776,53 +776,55 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_ackermann( _pose.translation()(0), _pose.translation()(1), _lookahead_distance - ); - - if (intersections.size() == 0) - { - // No intersections; head towards closest point on the path segment. - auto point_P = _pose.translation().head<2>(); - target = get_closest_point_on_line_segment(point_A, point_B, point_P); - } - else if (intersections.size() == 1) - { - target = intersections.at(0); - } - else if (intersections.size() == 2) - { - // Select the intersection closer to B. - auto distance_0 = (point_B.head<2>() - intersections.at(0)).norm(); - auto distance_1 = (point_B.head<2>() - intersections.at(1)).norm(); - target = intersections.at(distance_0 < distance_1 ? 0 : 1); - } + ); + + if (intersections.size() == 0) + { + // No intersections; head towards closest point on the path segment. + auto point_P = _pose.translation().head<2>(); + target = get_closest_point_on_line_segment(point_A, point_B, + point_P); + } + else if (intersections.size() == 1) + { + target = intersections.at(0); + } + else if (intersections.size() == 2) + { + // Select the intersection closer to B. + auto distance_0 = (point_B.head<2>() - intersections.at(0)).norm(); + auto distance_1 = (point_B.head<2>() - intersections.at(1)).norm(); + target = intersections.at(distance_0 < distance_1 ? 0 : 1); + } } _lookahead_point = Eigen::Vector3d(target(0), target(1), trajectory.at(_traj_wp_idx).pose.translation()(2)); - Eigen::Vector3d d_target = _lookahead_point - _pose.translation(); - - result.v = d_target.norm(); - - auto goal_heading = compute_heading(trajectory.at(_traj_wp_idx).pose); - double dir = 1.0; - result.w = compute_change_in_rotation( - current_heading, d_target, &goal_heading, &dir); - - // As turning yaw increases, slow down more. - double turning = fabs(result.w) / M_PI; - double slowdown = std::min(0.8, turning); // Minimum speed 20% - result.target_linear_speed_now = - std::min(result.max_speed.value(), _nominal_drive_speed) * - (1 - slowdown); - result.target_linear_speed_destination = result.target_linear_speed_now; - - if (_traj_wp_idx == trajectory.size() - 1 && - dpos_mag < _lookahead_distance) - { - // if near the last waypoint, slow to a stop nicely - result.target_linear_speed_destination = 0; - } + Eigen::Vector3d d_target = _lookahead_point - _pose.translation(); + + result.v = d_target.norm(); + + auto goal_heading = compute_heading(trajectory.at(_traj_wp_idx).pose); + double dir = 1.0; + result.w = compute_change_in_rotation( + current_heading, d_target, &goal_heading, &dir); + + // As turning yaw increases, slow down more. + double turning = fabs(result.w) / M_PI; + double slowdown = std::min(0.8, turning); // Minimum speed 20% + result.target_linear_speed_now = + std::min(result.max_speed.value(), _nominal_drive_speed) * + (1 - slowdown); + result.target_linear_speed_destination = + result.target_linear_speed_now; + + if (_traj_wp_idx == trajectory.size() - 1 && + dpos_mag < _lookahead_distance) + { + // if near the last waypoint, slow to a stop nicely + result.target_linear_speed_destination = 0; + } } } else @@ -861,10 +863,10 @@ bool SlotcarCommon::emergency_stop( // TODO get collision object name if (need_to_stop) RCLCPP_INFO_STREAM(logger(), "Stopping [" << _model_name << - "] to avoid a collision"); + "] to avoid a collision"); else RCLCPP_INFO_STREAM(logger(), "No more obstacles; resuming course for [" << - _model_name << "]"); + _model_name << "]"); } return _emergency_stop; diff --git a/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp b/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp index 566113c..1a78e66 100644 --- a/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp +++ b/rmf_robot_sim_gz_plugins/src/LightTuning/LightTuning.cpp @@ -44,25 +44,25 @@ std::string create_light_marker_str(const std::string& name, { std::ostringstream ss; ss << std::string( - "" - ""); + "" + ""); ss << "" << std::endl; ss << "" << pose << "" << std::endl; ss << std::string( - "true" - "" - "" - "false" - "0.5" - "" - "" - "0.5 0.5 0.5" - "" - "" - "" - "" - "" - ""); + "true" + "" + "" + "false" + "0.5" + "" + "" + "0.5 0.5 0.5" + "" + "" + "" + "" + "" + ""); return ss.str(); } @@ -186,7 +186,7 @@ std::ostream& operator<<(std::ostream& os, const sdf::Light& light) { os << " \n"; os << "" << (light.CastShadows() ? "true" : "false") << " \n"; diff --git a/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp b/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp index f276d1a..4dd81fa 100644 --- a/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp +++ b/rmf_robot_sim_gz_plugins/src/TeleportDispenser.cpp @@ -297,10 +297,10 @@ void TeleportDispenserPlugin::PreUpdate(const UpdateInfo& info, std::ref(ecm), std::placeholders::_1, _item_en); std::function check_filled_cb = [&]() - { - return _dispenser_vicinity_box.Contains( - ecm.Component(_item_en)->Data().Pos()); - }; + { + return _dispenser_vicinity_box.Contains( + ecm.Component(_item_en)->Data().Pos()); + }; _dispenser_common->on_update(fill_robot_list_cb, find_nearest_model_cb, place_on_entity_cb, check_filled_cb); diff --git a/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp b/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp index 293cf24..ac1c297 100644 --- a/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp +++ b/rmf_robot_sim_gz_plugins/src/TeleportIngestor.cpp @@ -239,7 +239,8 @@ void TeleportIngestorPlugin::init_non_static_models_poses( { if (!is_static->Data() && name->Data() != _ingestor_common->_guid) { - _ingestor_common->non_static_models_init_poses[name->Data()] = convert_pose( + _ingestor_common->non_static_models_init_poses[name->Data()] = + convert_pose( pose->Data()); } return true;