#include <motion_command_processor_base.h>

类 apollo::external_command::MotionCommandProcessorBase< T > 继承关系图:

apollo::external_command::MotionCommandProcessorBase< T > 的协作图:

Public 成员函数
	MotionCommandProcessorBase ()

	~MotionCommandProcessorBase () override=default

bool	Init (const std::shared_ptr< cyber::Node > &node) override

bool	GetCommandStatus (int64_t command_id, CommandStatus *status) const override
	Get the command status.

Public 成员函数继承自 apollo::external_command::CommandProcessorBase
	CommandProcessorBase ()

virtual	~CommandProcessorBase ()=default

Protected 成员函数
const std::shared_ptr< apollo::external_command::LaneWayTool > &	GetLaneWayTool () const

virtual bool	Convert (const std::shared_ptr< T > &command, std::shared_ptr< apollo::routing::RoutingRequest > &routing_request) const =0
	Convert moving command to RoutingRequest.

bool	SetStartPose (std::vector< apollo::routing::LaneWaypoint > *lane_way_points) const
	Set the start pose of lane_way_points with current vehicle pose.

bool	SetStartPose (std::shared_ptr< apollo::routing::RoutingRequest > &routing_request) const
	Set the start pose of RoutingRequest with current vehicle pose.

void	OnCommand (const std::shared_ptr< T > &command, std::shared_ptr< CommandStatus > &status)
	Process the incoming lane follow command.

virtual bool	ProcessSpecialCommand (const std::shared_ptr< T > &command, const std::shared_ptr< apollo::planning::PlanningCommand > &planning_command) const =0
	Process special command except RoutingRequest.

Protected 成员函数继承自 apollo::external_command::CommandProcessorBase
const CommandProcessorConfig &	GetProcessorConfig () const

const std::shared_ptr< cyber::Node > &	Node () const

Protected 属性
std::shared_ptr< cyber::Service< T, CommandStatus > >	command_service_

std::shared_ptr< WriterHandle >	planning_command_writer_

std::shared_ptr< WriterHandle >	routing_response_writer_

std::shared_ptr< apollo::routing::Routing >	routing_

std::shared_ptr< apollo::external_command::LaneWayTool >	lane_way_tool_

T	last_received_command_

MessageReader *	message_reader_

std::string	planning_command_status_name_

详细描述

template<typename T>
class apollo::external_command::MotionCommandProcessorBase< T >

在文件 motion_command_processor_base.h 第 54 行定义.

构造及析构函数说明

◆ MotionCommandProcessorBase()

template<typename T >

apollo::external_command::MotionCommandProcessorBase< T >::MotionCommandProcessorBase ( )

在文件 motion_command_processor_base.h 第 132 行定义.

    : command_service_(nullptr),
      planning_command_writer_(nullptr),
      routing_(std::make_shared<apollo::routing::Routing>()),
      lane_way_tool_(nullptr),
      message_reader_(MessageReader::Instance()) {
  last_received_command_.set_command_id(-1);
}

◆ ~MotionCommandProcessorBase()

template<typename T >

apollo::external_command::MotionCommandProcessorBase< T >::~MotionCommandProcessorBase ( )

overridedefault

成员函数说明

◆ Convert()

template<typename T >

virtual bool apollo::external_command::MotionCommandProcessorBase< T >::Convert	(	const std::shared_ptr< T > &	command,
		std::shared_ptr< apollo::routing::RoutingRequest > &	routing_request
	)		const

protectedpure virtual

Convert moving command to RoutingRequest.

参数

command	moving command to be converted.
routing_request	Convert result of RoutingRequest. If there is no LaneFollow action in moving command, the convert result can be nullptr.

返回: Return true if there is error occurs for converting.

◆ GetCommandStatus()

template<typename T >

bool apollo::external_command::MotionCommandProcessorBase< T >::GetCommandStatus	(	int64_t	command_id,
		CommandStatus *	status
	)		const

overridevirtual

Get the command status.

参数

command_id	Id of the command.
status	Output command status.

实现了 apollo::external_command::CommandProcessorBase.

在文件 motion_command_processor_base.h 第 187 行定义.

                                                     {
  CHECK_NOTNULL(status);
  if (last_received_command_.command_id() == command_id) {
    status->set_command_id(command_id);
    auto* latest_planning_command_status =
        message_reader_->GetMessage<CommandStatus>(
            planning_command_status_name_);
    if (nullptr == latest_planning_command_status) {
      status->set_status(apollo::external_command::CommandStatusType::ERROR);
      status->set_message("Cannot get planning command status!");
      return true;
    }
    status->CopyFrom(*latest_planning_command_status);
    return true;
  }
  return false;
}

◆ GetLaneWayTool()

template<typename T >

const std::shared_ptr< apollo::external_command::LaneWayTool > & apollo::external_command::MotionCommandProcessorBase< T >::GetLaneWayTool ( ) const

inlineprotected

在文件 motion_command_processor_base.h 第 70 行定义.

            {
    return lane_way_tool_;
  }

◆ Init()

template<typename T >

bool apollo::external_command::MotionCommandProcessorBase< T >::Init ( const std::shared_ptr< cyber::Node > & node )

overridevirtual

重载 apollo::external_command::CommandProcessorBase .

在文件 motion_command_processor_base.h 第 142 行定义.

                                          {
  // Init function of base class should be invoked first.
  if (!CommandProcessorBase::Init(node)) {
    AERROR << "MotionCommandProcessorBase init failed!";
    return false;
  }
  lane_way_tool_ =
      std::make_shared<apollo::external_command::LaneWayTool>(node);
  routing_->Init();
  // Create service for input command.
  const auto& config = GetProcessorConfig();
  command_service_ = node->CreateService<T, CommandStatus>(
      config.input_command_name(),
      [this](const std::shared_ptr<T>& command,
             std::shared_ptr<CommandStatus>& status) {
        this->OnCommand(command, status);
      });
 
  CHECK_GT(config.output_command_name().size(), 0);
  // Create writer for output command.
  apollo::cyber::proto::RoleAttributes attr;
  attr.set_channel_name(config.output_command_name().Get(0));
  auto qos = attr.mutable_qos_profile();
  qos->set_history(apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
  qos->set_reliability(
      apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
  qos->set_durability(
      apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
  auto message_writer = MessageWriter::Instance();
  planning_command_writer_ =
      message_writer->RegisterMessage<apollo::planning::PlanningCommand>(
          attr, config.planning_command_history_name());
  routing_response_writer_ =
      message_writer->RegisterMessage<apollo::routing::RoutingResponse>(
          FLAGS_routing_response_topic);
  // Create reader for input command status.
  CHECK_GT(config.input_command_status_name().size(), 0);
  planning_command_status_name_ = config.input_command_status_name().Get(0);
  message_reader_->RegisterMessage<CommandStatus>(
      planning_command_status_name_);
  return true;
}

◆ OnCommand()

template<typename T >

void apollo::external_command::MotionCommandProcessorBase< T >::OnCommand	(	const std::shared_ptr< T > &	command,
		std::shared_ptr< CommandStatus > &	status
	)

protected

Process the incoming lane follow command.

Search the routing to end point and send to planning module.

参数

command	Incoming lane follow command.
status	The command process result before sending to planning module.

在文件 motion_command_processor_base.h 第 207 行定义.

                                                                           {
  CHECK_NOTNULL(command);
  CHECK_NOTNULL(status);
  if (command->has_header()) {
    double timestamp = apollo::cyber::Clock::NowInSeconds();
    AINFO << std::setprecision(12) << "timestamp: " << timestamp << " "
          << "request for " << command->header().timestamp_sec();
    if (timestamp - command->header().timestamp_sec() > 2.0) {
      AINFO << "request for " << command->header().module_name()
            << " has been timeouted";
      return;
    }
  }
  last_received_command_.CopyFrom(*command);
  status->set_command_id(command->command_id());
  // Convert command to RoutingRequest.
  std::shared_ptr<apollo::routing::RoutingRequest> routing_request = nullptr;
  bool convert_result = Convert(command, routing_request);
  if (!convert_result) {
    status->set_status(CommandStatusType::ERROR);
    status->set_message("Cannot convert command to RoutingRequest: " +
                        command->DebugString());
    return;
  }
  // Search routing.
  std::string module_name = "UNKNOWN";
  if (command->has_header()) {
    module_name = command->header().module_name();
  }
  std::lock_guard<std::mutex> guard(this->mutex_);
  auto planning_command = std::make_shared<apollo::planning::PlanningCommand>();
  common::util::FillHeader(module_name, planning_command.get());
  if (nullptr != routing_request) {
    AINFO << "Process routing: " << routing_request->DebugString();
    auto routing_response =
        std::make_shared<apollo::routing::RoutingResponse>();
    if (!routing_->Process(routing_request, routing_response.get())) {
      status->set_status(CommandStatusType::ERROR);
      status->set_message("Cannot get routing of command: " +
                          command->DebugString());
      AERROR << "Routing process failed! ";
      return;
    }
    planning_command->mutable_lane_follow_command()->CopyFrom(
        *routing_response);
    // Publish routing response to display on HMI.
    routing_response_writer_->Write(routing_response);
  }
  // Process command except RoutingRequest.
  if (!ProcessSpecialCommand(command, planning_command)) {
    AERROR << "Process special command failed!";
    return;
  }
  planning_command->set_command_id(command->command_id());
  // Set target_speed.
  if (command->has_target_speed()) {
    planning_command->set_target_speed(command->target_speed());
  }
  planning_command->set_is_motion_command(true);
  // Send routing response to planning module.
  planning_command_writer_->Write(planning_command);
  AINFO << "publish: " << planning_command->DebugString();
  status->set_status(CommandStatusType::RUNNING);
}

◆ ProcessSpecialCommand()

template<typename T >

virtual bool apollo::external_command::MotionCommandProcessorBase< T >::ProcessSpecialCommand	(	const std::shared_ptr< T > &	command,
		const std::shared_ptr< apollo::planning::PlanningCommand > &	planning_command
	)		const

protectedpure virtual

Process special command except RoutingRequest.

参数

command	RoutingCommand to be converted.
planning_command	Output process result.

返回: True if no error occurs.

◆ SetStartPose() [1/2]

template<typename T >

bool apollo::external_command::MotionCommandProcessorBase< T >::SetStartPose ( std::shared_ptr< apollo::routing::RoutingRequest > & routing_request ) const

protected

Set the start pose of RoutingRequest with current vehicle pose.

参数

routing_request RoutingRequest to be updated.

返回: Return true if there is error occurs.

在文件 motion_command_processor_base.h 第 286 行定义.

                                                                       {
  CHECK_NOTNULL(routing_request);
  // Get the current vehicle pose as start pose.
  auto start_pose = routing_request->add_waypoint();
  if (!lane_way_tool_->GetVehicleLaneWayPoint(start_pose)) {
    AERROR << "Get lane near start pose failed!";
    return false;
  }
  return true;
}

◆ SetStartPose() [2/2]

template<typename T >

bool apollo::external_command::MotionCommandProcessorBase< T >::SetStartPose ( std::vector< apollo::routing::LaneWaypoint > * lane_way_points ) const

protected

Set the start pose of lane_way_points with current vehicle pose.

参数

lane_way_points start pose to be updated.

返回: Return true if there is error occurs.

在文件 motion_command_processor_base.h 第 274 行定义.

                                                                 {
  CHECK_NOTNULL(lane_way_points);
  // Get the current vehicle pose as start pose.
  if (!lane_way_tool_->GetVehicleLaneWayPoints(lane_way_points)) {
    AERROR << "Get lane near start pose failed!";
    return false;
  }
  return true;
}