Skip to content

Commit

Permalink
refactor(vehicle_cmd_gate): rename adapi pause interface (autowarefou…
Browse files Browse the repository at this point in the history
…ndation#3794)

* refactor(vehicle_cmd_gate): rename adapi pause interface

Signed-off-by: Takagi, Isamu <[email protected]>

* refactor: fix internal name

Signed-off-by: Takagi, Isamu <[email protected]>

---------

Signed-off-by: Takagi, Isamu <[email protected]>
  • Loading branch information
isamu-takagi authored Jun 8, 2023
1 parent 3027968 commit 69e89d9
Show file tree
Hide file tree
Showing 5 changed files with 22 additions and 22 deletions.
2 changes: 1 addition & 1 deletion control/vehicle_cmd_gate/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ autoware_package()
ament_auto_add_library(vehicle_cmd_gate_node SHARED
src/vehicle_cmd_gate.cpp
src/vehicle_cmd_filter.cpp
src/pause_interface.cpp
src/adapi_pause_interface.cpp
src/moderate_stop_interface.cpp
)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "pause_interface.hpp"
#include "adapi_pause_interface.hpp"

namespace vehicle_cmd_gate
{

PauseInterface::PauseInterface(rclcpp::Node * node) : node_(node)
AdapiPauseInterface::AdapiPauseInterface(rclcpp::Node * node) : node_(node)
{
const auto adaptor = component_interface_utils::NodeAdaptor(node);
adaptor.init_srv(srv_set_pause_, this, &PauseInterface::on_pause);
adaptor.init_srv(srv_set_pause_, this, &AdapiPauseInterface::on_pause);
adaptor.init_pub(pub_is_paused_);
adaptor.init_pub(pub_is_start_requested_);

Expand All @@ -29,12 +29,12 @@ PauseInterface::PauseInterface(rclcpp::Node * node) : node_(node)
publish();
}

bool PauseInterface::is_paused()
bool AdapiPauseInterface::is_paused()
{
return is_paused_;
}

void PauseInterface::publish()
void AdapiPauseInterface::publish()
{
if (prev_is_paused_ != is_paused_) {
IsPaused::Message msg;
Expand All @@ -53,12 +53,12 @@ void PauseInterface::publish()
}
}

void PauseInterface::update(const AckermannControlCommand & control)
void AdapiPauseInterface::update(const AckermannControlCommand & control)
{
is_start_requested_ = eps < std::abs(control.longitudinal.speed);
}

void PauseInterface::on_pause(
void AdapiPauseInterface::on_pause(
const SetPause::Service::Request::SharedPtr req, const SetPause::Service::Response::SharedPtr res)
{
is_paused_ = req->pause;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PAUSE_INTERFACE_HPP_
#define PAUSE_INTERFACE_HPP_
#ifndef ADAPI_PAUSE_INTERFACE_HPP_
#define ADAPI_PAUSE_INTERFACE_HPP_

#include <component_interface_specs/control.hpp>
#include <component_interface_utils/rclcpp.hpp>
Expand All @@ -24,7 +24,7 @@
namespace vehicle_cmd_gate
{

class PauseInterface
class AdapiPauseInterface
{
private:
static constexpr double eps = 1e-3;
Expand All @@ -34,7 +34,7 @@ class PauseInterface
using IsStartRequested = control_interface::IsStartRequested;

public:
explicit PauseInterface(rclcpp::Node * node);
explicit AdapiPauseInterface(rclcpp::Node * node);
bool is_paused();
void publish();
void update(const AckermannControlCommand & control);
Expand All @@ -57,4 +57,4 @@ class PauseInterface

} // namespace vehicle_cmd_gate

#endif // PAUSE_INTERFACE_HPP_
#endif // ADAPI_PAUSE_INTERFACE_HPP_
14 changes: 7 additions & 7 deletions control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -197,7 +197,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
updater_.add("emergency_stop_operation", this, &VehicleCmdGate::checkExternalEmergencyStop);

// Pause interface
pause_ = std::make_unique<PauseInterface>(this);
adapi_pause_ = std::make_unique<AdapiPauseInterface>(this);
moderate_stop_interface_ = std::make_unique<ModerateStopInterface>(this);

// Timer
Expand Down Expand Up @@ -399,9 +399,9 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
filtered_commands.control = createStopControlCmd();
}

// Check pause
pause_->update(filtered_commands.control);
if (pause_->is_paused()) {
// Check pause. Place this check after all other checks as it needs the final output.
adapi_pause_->update(filtered_commands.control);
if (adapi_pause_->is_paused()) {
filtered_commands.control.longitudinal.speed = 0.0;
filtered_commands.control.longitudinal.acceleration = stop_hold_acceleration_;
}
Expand All @@ -417,7 +417,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
// Publish commands
vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
control_cmd_pub_->publish(filtered_commands.control);
pause_->publish();
adapi_pause_->publish();
moderate_stop_interface_->publish();

// Save ControlCmd to steering angle when disengaged
Expand All @@ -434,7 +434,7 @@ void VehicleCmdGate::publishEmergencyStopControlCommands()
control_cmd = createEmergencyStopControlCmd();

// Update control command
pause_->update(control_cmd);
adapi_pause_->update(control_cmd);

// gear
GearCommand gear;
Expand Down Expand Up @@ -482,7 +482,7 @@ void VehicleCmdGate::publishStatus()
engage_pub_->publish(autoware_engage);
pub_external_emergency_->publish(external_emergency);
operation_mode_pub_->publish(current_operation_mode_);
pause_->publish();
adapi_pause_->publish();
moderate_stop_interface_->publish();
}

Expand Down
4 changes: 2 additions & 2 deletions control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,8 @@
#ifndef VEHICLE_CMD_GATE_HPP_
#define VEHICLE_CMD_GATE_HPP_

#include "adapi_pause_interface.hpp"
#include "moderate_stop_interface.hpp"
#include "pause_interface.hpp"
#include "vehicle_cmd_filter.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>
Expand Down Expand Up @@ -217,7 +217,7 @@ class VehicleCmdGate : public rclcpp::Node
VehicleCmdFilter filter_on_transition_;

// Pause interface for API
std::unique_ptr<PauseInterface> pause_;
std::unique_ptr<AdapiPauseInterface> adapi_pause_;
std::unique_ptr<ModerateStopInterface> moderate_stop_interface_;
};

Expand Down

0 comments on commit 69e89d9

Please sign in to comment.