From b5b30d4b20ae14b3deafdf8a139ce6c5d8d94bcd Mon Sep 17 00:00:00 2001 From: Jiangtao Hu Date: Mon, 5 Mar 2018 23:18:39 -0800 Subject: [PATCH] dreamview: add sim control initial support in navigation mode. still need more code to make it work. --- modules/dreamview/backend/sim_control/BUILD | 1 + .../backend/sim_control/sim_control.cc | 33 +++++++++++++++++-- .../backend/sim_control/sim_control.h | 7 ++++ .../backend/sim_control/sim_control_test.cc | 8 +++++ modules/map/relative_map/navigation_lane.cc | 6 ++-- 5 files changed, 50 insertions(+), 5 deletions(-) diff --git a/modules/dreamview/backend/sim_control/BUILD b/modules/dreamview/backend/sim_control/BUILD index 11fe0ccd9f..e22e12c7f9 100644 --- a/modules/dreamview/backend/sim_control/BUILD +++ b/modules/dreamview/backend/sim_control/BUILD @@ -15,6 +15,7 @@ cc_library( "//modules/common/adapters:adapter_manager", "//modules/dreamview/backend/common:dreamview_gflags", "//modules/dreamview/backend/map:map_service", + "//modules/map/relative_map/proto:navigation_proto", "@gtest//:gtest", ], ) diff --git a/modules/dreamview/backend/sim_control/sim_control.cc b/modules/dreamview/backend/sim_control/sim_control.cc index f5e0ba9abb..494e5dce1e 100644 --- a/modules/dreamview/backend/sim_control/sim_control.cc +++ b/modules/dreamview/backend/sim_control/sim_control.cc @@ -75,11 +75,14 @@ void SimControl::Init(bool set_start_point, double start_velocity, AdapterManager::AddPlanningCallback(&SimControl::OnPlanning, this); AdapterManager::AddRoutingResponseCallback(&SimControl::OnRoutingResponse, this); + AdapterManager::AddNavigationCallback(&SimControl::OnReceiveNavigationInfo, + this); + // Start timer to publish localization and chassis messages. sim_control_timer_ = AdapterManager::CreateTimer( ros::Duration(kSimControlInterval), &SimControl::TimerCallback, this); - if (set_start_point) { + if (set_start_point && !FLAGS_use_navigation_mode) { apollo::common::PointENU start_point; if (!map_service_->GetStartPoint(&start_point)) { AWARN << "Failed to get a dummy start point from map!"; @@ -92,6 +95,17 @@ void SimControl::Init(bool set_start_point, double start_velocity, start_acceleration_ = start_acceleration; } +void SimControl::OnReceiveNavigationInfo( + const relative_map::NavigationInfo& navigation_info) { + navigation_info_ = navigation_info; + if (navigation_info_.navigation_path_size() > 0) { + const auto& path = navigation_info_.navigation_path(0).path(); + if (path.path_point_size() > 0) { + adc_position_ = path.path_point(0); + } + } +} + void SimControl::SetStartPoint(const double x, const double y) { next_point_.set_v(start_velocity_); next_point_.set_a(start_acceleration_); @@ -291,11 +305,22 @@ void SimControl::PublishLocalization(double lambda) { pose->mutable_position()->set_y(cur_y); double cur_z = Interpolate(prev.z(), next.z(), lambda); pose->mutable_position()->set_z(cur_z); - // Set orientation and heading double cur_theta = NormalizeAngle( prev.theta() + lambda * NormalizeAngle(next.theta() - prev.theta())); + if (FLAGS_use_navigation_mode) { + double flu_x = cur_x; + double flu_y = cur_y; + double enu_x = 0.0; + double enu_y = 0.0; + common::math::RotateAxis(-cur_theta, flu_x, flu_y, &enu_x, &enu_y); + enu_x += adc_position_.x(); + enu_y += adc_position_.y(); + pose->mutable_position()->set_x(enu_x); + pose->mutable_position()->set_y(enu_y); + } + Eigen::Quaternion cur_orientation = HeadingToQuaternion(cur_theta); pose->mutable_orientation()->set_qw(cur_orientation.w()); @@ -333,6 +358,10 @@ void SimControl::PublishLocalization(double lambda) { pose->mutable_linear_acceleration_vrf()); AdapterManager::PublishLocalization(localization); + + adc_position_.set_x(pose->position().x()); + adc_position_.set_y(pose->position().y()); + adc_position_.set_z(pose->position().z()); } } // namespace dreamview diff --git a/modules/dreamview/backend/sim_control/sim_control.h b/modules/dreamview/backend/sim_control/sim_control.h index b56bd68398..9b58f8006e 100644 --- a/modules/dreamview/backend/sim_control/sim_control.h +++ b/modules/dreamview/backend/sim_control/sim_control.h @@ -28,6 +28,7 @@ #include "modules/dreamview/backend/common/dreamview_gflags.h" #include "modules/dreamview/backend/map/map_service.h" #include "modules/dreamview/backend/sim_control/sim_control_interface.h" +#include "modules/map/relative_map/proto/navigation.pb.h" /** * @namespace apollo::dreamview @@ -85,6 +86,8 @@ class SimControl : SimControlInterface { private: void OnPlanning(const apollo::planning::ADCTrajectory &trajectory); void OnRoutingResponse(const apollo::routing::RoutingResponse &routing); + void OnReceiveNavigationInfo( + const relative_map::NavigationInfo &navigation_info); // Reset the start point, which can be a dummy point on the map or received // from the routing module. @@ -140,12 +143,16 @@ class SimControl : SimControlInterface { apollo::common::TrajectoryPoint prev_point_; apollo::common::TrajectoryPoint next_point_; + common::PathPoint adc_position_; + // Initial velocity and acceleration of the main vehicle double start_velocity_ = 0.0; double start_acceleration_ = 0.0; static constexpr int kPlanningCountToStart = 5; + relative_map::NavigationInfo navigation_info_; + FRIEND_TEST(SimControlTest, Test); }; diff --git a/modules/dreamview/backend/sim_control/sim_control_test.cc b/modules/dreamview/backend/sim_control/sim_control_test.cc index dece341731..277fdad29e 100644 --- a/modules/dreamview/backend/sim_control/sim_control_test.cc +++ b/modules/dreamview/backend/sim_control/sim_control_test.cc @@ -80,6 +80,14 @@ class SimControlTest : public ::testing::Test { apollo::common::adapter::AdapterConfig::ROUTING_RESPONSE); } + { + auto *sub_config = config.add_config(); + sub_config->set_mode( + apollo::common::adapter::AdapterConfig::RECEIVE_ONLY); + sub_config->set_type( + apollo::common::adapter::AdapterConfig::NAVIGATION); + } + AdapterManager::Init(config); sim_control_->Start(); diff --git a/modules/map/relative_map/navigation_lane.cc b/modules/map/relative_map/navigation_lane.cc index af62607694..545fedf304 100644 --- a/modules/map/relative_map/navigation_lane.cc +++ b/modules/map/relative_map/navigation_lane.cc @@ -160,12 +160,12 @@ void NavigationLane::ConvertNavigationLineToPath(common::Path *path) { point->CopyFrom(navigation_path.path_point(i)); // shift to (0, 0) - double emu_x = point->x() + dx; - double emu_y = point->y() + dy; + double enu_x = point->x() + dx; + double enu_y = point->y() + dy; double flu_x = 0.0; double flu_y = 0.0; - common::math::RotateAxis(original_pose_.heading(), emu_x, emu_y, &flu_x, + common::math::RotateAxis(original_pose_.heading(), enu_x, enu_y, &flu_x, &flu_y); point->set_x(flu_x); -- GitLab