Fix style

Signed-off-by: Luca Della Vedova <[email protected]>

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<components::JointPositionReset>(joint_entity, components::JointPositionReset(
+        ecm.CreateComponent<components::JointPositionReset>(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<components::DoorCmd>(entity,
           components::DoorCmd(door_command));

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;
             }
           }

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)
   {

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;

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(
-      "<?xml version=\"1.0\"?>"
-      "<sdf version=\"1.7\">");
+    "<?xml version=\"1.0\"?>"
+    "<sdf version=\"1.7\">");
   ss << "<model name=\"" << name << "\">" << std::endl;
   ss << "<pose>" << pose << "</pose>" << std::endl;
   ss << std::string(
-      "<static>true</static>"
-      "<link name=\"box_link\">"
-      "<visual name=\"box_visual\">"
-      "<cast_shadows>false</cast_shadows>"
-      "<transparency>0.5</transparency>"
-      "<geometry>"
-      "<box>"
-      "<size>0.5 0.5 0.5</size>"
-      "</box>"
-      "</geometry>"
-      "</visual>"
-      "</link>"
-      "</model>"
-      "</sdf>");
+    "<static>true</static>"
+    "<link name=\"box_link\">"
+    "<visual name=\"box_visual\">"
+    "<cast_shadows>false</cast_shadows>"
+    "<transparency>0.5</transparency>"
+    "<geometry>"
+    "<box>"
+    "<size>0.5 0.5 0.5</size>"
+    "</box>"
+    "</geometry>"
+    "</visual>"
+    "</link>"
+    "</model>"
+    "</sdf>");
   return ss.str();
 }
 
@@ -186,7 +186,7 @@ std::ostream& operator<<(std::ostream& os, const sdf::Light& light)
 {
   os << "<light type=\""
      << (light.Type() == sdf::LightType::POINT ? "point" :
-    (light.Type() == sdf::LightType::DIRECTIONAL ? "directional" : "spot"))
+  (light.Type() == sdf::LightType::DIRECTIONAL ? "directional" : "spot"))
      << "\" name=\"" << light.Name() << "\"> \n";
   os << "<cast_shadows>"
      << (light.CastShadows() ? "true" : "false") << "</cast_shadows> \n";

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<bool(void)> check_filled_cb = [&]()
-    {
-      return _dispenser_vicinity_box.Contains(
-        ecm.Component<components::Pose>(_item_en)->Data().Pos());
-    };
+  {
+    return _dispenser_vicinity_box.Contains(
+      ecm.Component<components::Pose>(_item_en)->Data().Pos());
+  };
 
   _dispenser_common->on_update(fill_robot_list_cb, find_nearest_model_cb,
     place_on_entity_cb, check_filled_cb);

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;