提交 9d5c4ee2 编写于 作者: H henryhu6 提交者: Jiangtao Hu

clean up routing 2

上级 aed72cc6
config {
type: LOCALIZATION
type: ROUTING_REQUEST
mode: RECEIVE_ONLY
message_history_limit: 10
}
config {
type: PAD
mode: RECEIVE_ONLY
message_history_limit: 10
}
config {
type: PLANNING_TRAJECTORY
mode: RECEIVE_ONLY
message_history_limit: 10
}
config {
type: CHASSIS
mode: RECEIVE_ONLY
message_history_limit: 10
}
config {
type: CONTROL_COMMAND
type: ROUTING
mode: PUBLISH_ONLY
}
config {
......
此差异已折叠。
......@@ -22,60 +22,46 @@
namespace apollo {
namespace routing {
Routing::Routing() {
_node_handle_ptr.reset(new ros::NodeHandle(FLAGS_node_namespace));
ROS_INFO("Use new routing request and result!");
_service = _node_handle_ptr->advertiseService(FLAGS_signal_probe_service,
&Routing::on_request, this);
_publisher = _node_handle_ptr->advertise<std_msgs::String>(
FLAGS_route_topic_for_broadcast, 1);
std::string Routing::Name() const { return FLAGS_node_name; }
Status Routing::Init() {
std::string graph_path = FLAGS_graph_dir + "/" + FLAGS_graph_file_name;
ROS_INFO("Use routing topology graph path: %s", graph_path.c_str());
_navigator_ptr.reset(new Navigator(graph_path));
// set init ok for ADS test, when it knows routing is ok, it can replay bags
_node_handle_ptr->setParam(FLAGS_rosparam_name_routing_init_status, true);
return Status::OK();
}
Routing::~Routing() {}
Status Control::Start() {
if (!_navigator_ptr->is_ready()) {
AERROR << "Navigator is not ready!";
return Status::ERROR();
}
ROS_INFO("Routing service is ready.");
apollo::common::monitor::MonitorBuffer buffer(&monitor_);
buffer.INFO("Routing started");
return Status::OK();
}
bool Routing::on_request(routing::routing_signal::Request& req,
routing::routing_signal::Response& res) {
ROS_INFO("Get new routing request!!!");
void OnRouting_Request(const apollo::routing::RoutingRequest &routing_req) {
AINFO << "Get new routing request!!!";
::apollo::routing::RoutingRequest request_proto;
::apollo::routing::RoutingResult response_proto;
if (!request_proto.ParseFromString(req.routing_request.data)) {
ROS_ERROR("The request proto is invalid.");
AERROR << "The request proto is invalid.";
return false;
}
if (!_navigator_ptr->search_route(request_proto, &response_proto)) {
ROS_ERROR("Failed to search route with navigator.");
return false;
}
if (!response_proto.SerializeToString(&(res.routing_response.data))) {
ROS_ERROR("Failed to serialize routing response.");
AERROR<< "Failed to search route with navigator.";
return false;
}
if (request_proto.broadcast()) {
std_msgs::String publish_msg;
if (!response_proto.SerializeToString(&(publish_msg.data))) {
ROS_ERROR("Failed to serialize routing response.");
return false;
}
_publisher.publish(publish_msg);
AdapterManager::PublishControlCommand(*control_command);
}
return true;
}
bool Routing::run() {
if (!_navigator_ptr->is_ready()) {
ROS_ERROR("Navigator is not ready!");
return false;
}
ROS_INFO("Routing service is ready.");
ros::spin();
return false;
}
void Routing::Stop() {}
} // namespace routing
} // namespace apollo
......@@ -19,31 +19,55 @@
#include <memory>
#include "routing/routing_signal.h"
#include "common/routing_gflags.h"
#include "common/routing_macros.h"
#include "modules/common/apollo_app.h"
#include "modules/common/monitor/monitor.h"
namespace apollo {
namespace routing {
class Navigator;
class Routing : public apollo::common::ApolloApp {
friend class RoutingTestBase;
// friend class RoutingTestBase;
public:
~Routing();
bool run();
Routing() : monitor_(apollo::common::monitor::MonitorMessageItem::Routing) {}
/**
* @brief module name
*/
std::string Name() const override;
/**
* @brief module initialization function
* @return initialization status
*/
apollo::common::Status Init() override;
/**
* @brief module start function
* @return start status
*/
apollo::common::Status Start() override;
/**
* @brief module stop function
*/
void Stop() override;
/**
* @brief destructor
*/
virtual ~Routing() = default;
private:
bool on_request(routing::routing_signal::Request& req,
routing::routing_signal::Response& res);
void OnRouting_Request(const apollo::routing::RoutingRequest &routing_req);
private:
std::unique_ptr<ros::NodeHandle> _node_handle_ptr;
ros::ServiceServer _service;
ros::Publisher _publisher;
std::unique_ptr<Navigator> _navigator_ptr;
DECLARE_ARBITER_SINGLETON(Routing);
};
} // namespace routing
......
std_msgs/String routing_request
---
std_msgs/String routing_response
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "edge_creator",
srcs = [
"edge_creator.cc",
],
hdrs = [
"edge_creator.h",
],
deps = [
"//external:gflags",
],
)
cc_library(
name = "graph_creator",
srcs = [
"graph_creator.cc",
],
hdrs = [
"graph_creator.h",
],
deps = [
"//external:gflags",
":edge_creator",
":node_creator",
],
)
cc_library(
name = "node_creator",
srcs = [
"node_creator.cc",
],
hdrs = [
"node_creator.h",
],
deps = [
"//external:gflags",
],
)
cc_binary(
name = "topo_creator",
srcs = ["topo_creator.cc"],
deps = [
"//external:gflags",
":graph_creator",
"//modules/common:log",
"//modules/routing/common",
"//modules/control/proto:control_proto",
],
)
cpplint()
......@@ -19,7 +19,7 @@
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "topo_creator/graph_creator.h"
#include "modules/routing/topo_creator/graph_creator.h"
DEFINE_string(base_map_dir, "/home/caros/adu_data/map",
"directory of base map");
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册