From 5d2d176457b932c32dbbf2b8e40b328bea085532 Mon Sep 17 00:00:00 2001 From: LitoNeo Date: Mon, 5 Jul 2021 23:02:40 +0800 Subject: [PATCH] bugfix: fix lane length display error --- .../rviz/rviz_config_traj_gene.rviz | 57 +-- .../trajectory_generator/src/traj_extract.cpp | 395 ++++++------------ 2 files changed, 167 insertions(+), 285 deletions(-) diff --git a/map_tools/modules/trajectory_generator/rviz/rviz_config_traj_gene.rviz b/map_tools/modules/trajectory_generator/rviz/rviz_config_traj_gene.rviz index 657bae6..5ec3394 100644 --- a/map_tools/modules/trajectory_generator/rviz/rviz_config_traj_gene.rviz +++ b/map_tools/modules/trajectory_generator/rviz/rviz_config_traj_gene.rviz @@ -6,8 +6,8 @@ Panels: Expanded: - /Global Options1 - /Status1 - Splitter Ratio: 0.558333337 - Tree Height: 775 + Splitter Ratio: 0.5583333373069763 + Tree Height: 728 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -16,7 +16,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -26,7 +26,9 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: map + SyncSource: "" +Preferences: + PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -38,7 +40,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -73,7 +75,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.0299999993 + Size (m): 0.029999999329447746 Style: Flat Squares Topic: /map/full Unreliable: false @@ -85,11 +87,11 @@ Visualization Manager: Class: rviz/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 Line Style: Lines - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Name: current_path Offset: X: 0 @@ -97,9 +99,9 @@ Visualization Manager: Z: 0 Pose Color: 255; 85; 255 Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /current_path Unreliable: false Value: true @@ -116,7 +118,7 @@ Visualization Manager: Marker Topic: /path/trajectory_defined Name: finished_path Namespaces: - "": true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -124,7 +126,7 @@ Visualization Manager: Marker Topic: /path/label Name: finished_path_label Namespaces: - "": true + {} Queue Size: 100 Value: true Enabled: true @@ -142,7 +144,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -151,29 +156,29 @@ Visualization Manager: Value: true Views: Current: - Angle: -0.0299997889 + Angle: -0.029999788850545883 Class: rviz/TopDownOrtho Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Scale: 4.78209257 + Near Clip Distance: 0.009999999776482582 + Scale: 4.782092571258545 Target Frame: Value: TopDownOrtho (rviz) - X: 8.99983788 - Y: 5.06838322 + X: 8.999837875366211 + Y: 5.06838321685791 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 1025 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000029d00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003c80000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -182,6 +187,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 1920 - Y: -4 + Width: 1853 + X: 67 + Y: 27 diff --git a/map_tools/modules/trajectory_generator/src/traj_extract.cpp b/map_tools/modules/trajectory_generator/src/traj_extract.cpp index 190f1bd..bd15d4e 100644 --- a/map_tools/modules/trajectory_generator/src/traj_extract.cpp +++ b/map_tools/modules/trajectory_generator/src/traj_extract.cpp @@ -1,51 +1,44 @@ #include "traj_extract.h" + +#include #include #include -#include // #include // #include -#include // 实现sensor_msgs和pcl::PointCloud的转换 +#include // 实现sensor_msgs和pcl::PointCloud的转换 //--------------- public // !! static 变量需要在cpp中进行初始化声明才能使用,否则报undefined reference错误 smartcar::tools::Traj_Extractor *smartcar::tools::Traj_Extractor::instance = 0; -smartcar::tools::Traj_Extractor & -smartcar::tools::Traj_Extractor::getInstance() -{ - if (!instance) - { +smartcar::tools::Traj_Extractor &smartcar::tools::Traj_Extractor::getInstance() { + if (!instance) { instance = new Traj_Extractor; } return *instance; } -bool smartcar::tools::Traj_Extractor::launch_thread(const std::string &input_folder, const std::string &output_folder) -{ +bool smartcar::tools::Traj_Extractor::launch_thread(const std::string &input_folder, const std::string &output_folder) { set_folder(input_folder, output_folder); init_ros(); load_map(); ros::spin(); } -bool smartcar::tools::Traj_Extractor::set_folder(const std::string &input_folder, const std::string &output_folder) -{ +bool smartcar::tools::Traj_Extractor::set_folder(const std::string &input_folder, const std::string &output_folder) { _input_folder = input_folder; _output_folder = output_folder; - if (!smartcar::utils::endswith(input_folder, "/")) - _input_folder += "/"; - if (!smartcar::utils::endswith(output_folder, "/")) - _output_folder += "/"; + if (!smartcar::utils::endswith(input_folder, "/")) _input_folder += "/"; + if (!smartcar::utils::endswith(output_folder, "/")) _output_folder += "/"; } -bool smartcar::tools::Traj_Extractor::adjust_orientation(){ +bool smartcar::tools::Traj_Extractor::adjust_orientation() { int size = current_path.poses.size(); - if (size < 2){ + if (size < 2) { return false; } // 调整每个轨迹点的角度 - for (size_t i = 0; i < size - 1; i++) - { + for (size_t i = 0; i < size - 1; i++) { geometry_msgs::PoseStamped p_c = current_path.poses[i]; geometry_msgs::PoseStamped p_n = current_path.poses[i + 1]; double yaw = std::atan2(p_n.pose.position.y - p_c.pose.position.y, p_n.pose.position.x - p_c.pose.position.x); @@ -58,23 +51,18 @@ bool smartcar::tools::Traj_Extractor::adjust_orientation(){ current_path.poses[i].pose.orientation.z = q.z(); current_path.poses[i].pose.orientation.w = q.w(); } - current_path.poses[size-1].pose.orientation.x = current_path.poses[size-2].pose.orientation.x; - current_path.poses[size-1].pose.orientation.y = current_path.poses[size-2].pose.orientation.x; - current_path.poses[size-1].pose.orientation.z = current_path.poses[size-2].pose.orientation.x; - current_path.poses[size-1].pose.orientation.w = current_path.poses[size-2].pose.orientation.x; + current_path.poses[size - 1].pose.orientation.x = current_path.poses[size - 2].pose.orientation.x; + current_path.poses[size - 1].pose.orientation.y = current_path.poses[size - 2].pose.orientation.x; + current_path.poses[size - 1].pose.orientation.z = current_path.poses[size - 2].pose.orientation.x; + current_path.poses[size - 1].pose.orientation.w = current_path.poses[size - 2].pose.orientation.x; return true; } -bool smartcar::tools::Traj_Extractor::create_trajtory(traj_info &traj_info) -{ - return on_create_trajectory(traj_info); -} +bool smartcar::tools::Traj_Extractor::create_trajtory(traj_info &traj_info) { return on_create_trajectory(traj_info); } -bool smartcar::tools::Traj_Extractor::delete_one_point() -{ - if (current_path.poses.empty()) - return false; +bool smartcar::tools::Traj_Extractor::delete_one_point() { + if (current_path.poses.empty()) return false; current_path.poses.pop_back(); pub_path.publish(current_path); marker_start.pose = current_path.poses[current_path.poses.size() - 1].pose; @@ -82,19 +70,12 @@ bool smartcar::tools::Traj_Extractor::delete_one_point() return true; } -bool smartcar::tools::Traj_Extractor::write_back() -{ - return on_write_back(); -} +bool smartcar::tools::Traj_Extractor::write_back() { return on_write_back(); } -void smartcar::tools::Traj_Extractor::reload_map() -{ - load_map(); -} +void smartcar::tools::Traj_Extractor::reload_map() { load_map(); } // ------------- private` -bool smartcar::tools::Traj_Extractor::load_map() -{ +bool smartcar::tools::Traj_Extractor::load_map() { pcl::PointCloud::Ptr cloud = load_pcd_folder(this->_input_folder); sensor_msgs::PointCloud2 msg_cloud; pcl::toROSMsg(*cloud, msg_cloud); @@ -105,11 +86,11 @@ bool smartcar::tools::Traj_Extractor::load_map() return true; } -void smartcar::tools::Traj_Extractor::init_ros() -{ +void smartcar::tools::Traj_Extractor::init_ros() { ros::NodeHandle nh; current_path.header.frame_id = "map"; - sub_pose = nh.subscribe("/initialpose", 1, boost::bind(&smartcar::tools::Traj_Extractor::pose_cb, this, _1)); + sub_pose = + nh.subscribe("/initialpose", 1, boost::bind(&smartcar::tools::Traj_Extractor::pose_cb, this, _1)); pub_path = nh.advertise("/current_path", 1); pub_markerArray = nh.advertise("/path/trajectory_defined", 10); pub_texts = nh.advertise("/path/label", 1); @@ -130,13 +111,10 @@ void smartcar::tools::Traj_Extractor::init_ros() marker_start.color.a = 1; } -bool smartcar::tools::Traj_Extractor::pose_cb( - const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) -{ +bool smartcar::tools::Traj_Extractor::pose_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) { marker_start.pose = msg->pose.pose; pub_marker_start.publish(marker_start); - if (is_begin) - { + if (is_begin) { current_path.poses.clear(); is_begin = false; } @@ -153,38 +131,30 @@ bool smartcar::tools::Traj_Extractor::pose_cb( return true; } -bool smartcar::tools::Traj_Extractor::insert_pose( - const geometry_msgs::PoseStamped pose) -{ - if (current_path.poses.empty()) - { +bool smartcar::tools::Traj_Extractor::insert_pose(const geometry_msgs::PoseStamped pose) { + if (current_path.poses.empty()) { current_path.poses.push_back(pose); return true; } int size = current_path.poses.size(); geometry_msgs::PoseStamped start = current_path.poses[size - 1]; geometry_msgs::PoseStamped end = pose; - if (distance2points(start, end) <= 1.0) - { + if (distance2points(start, end) <= 1.0) { current_path.poses.push_back(pose); return true; } double lenx = end.pose.position.x - start.pose.position.x; double leny = end.pose.position.y - start.pose.position.y; int cnt; - if (std::abs(lenx) > std::abs(leny)) - { + if (std::abs(lenx) > std::abs(leny)) { cnt = int(std::abs(lenx)) * 3; - } - else - { + } else { cnt = int(std::abs(leny)) * 3; } double dx = lenx / cnt; double dy = leny / cnt; - for (int i = 1; i <= cnt; i++) - { + for (int i = 1; i <= cnt; i++) { geometry_msgs::PoseStamped temp; temp.pose.position.x = start.pose.position.x + i * dx; temp.pose.position.y = start.pose.position.y + i * dy; @@ -194,17 +164,12 @@ bool smartcar::tools::Traj_Extractor::insert_pose( return true; } -bool smartcar::tools::Traj_Extractor::smooth_path(double weight_data, - double weight_smooth, - double tolerance) -{ +bool smartcar::tools::Traj_Extractor::smooth_path(double weight_data, double weight_smooth, double tolerance) { int size = current_path.poses.size(); int count[size + 1]; - for (size_t i = 0; i <= size; i++) - count[i] = 0; + for (size_t i = 0; i <= size; i++) count[i] = 0; - if (current_path.poses.size() <= 2) - { + if (current_path.poses.size() <= 2) { // cout << "Can't Smooth Path, origin_path Size=" << path.size() << endl; return true; } @@ -214,21 +179,23 @@ bool smartcar::tools::Traj_Extractor::smooth_path(double weight_data, double xtemp, ytemp; int nIterations = 0; - while (change >= _tolerance) - { + while (change >= _tolerance) { change = 0.0; - for (int i = std::max(size - 50, 1); i < size - 1; i++) // + for (int i = std::max(size - 50, 1); i < size - 1; i++) // { - if (count[i] > 20) - continue; + if (count[i] > 20) continue; xtemp = current_path.poses[i].pose.position.x; ytemp = current_path.poses[i].pose.position.y; current_path.poses[i].pose.position.x += weight_data * (origin_path.poses[i].pose.position.x - current_path.poses[i].pose.position.x); current_path.poses[i].pose.position.y += weight_data * (origin_path.poses[i].pose.position.y - current_path.poses[i].pose.position.y); - current_path.poses[i].pose.position.x += weight_smooth * (current_path.poses[i - 1].pose.position.x + current_path.poses[i + 1].pose.position.x - (2.0 * current_path.poses[i].pose.position.x)); - current_path.poses[i].pose.position.y += weight_smooth * (current_path.poses[i - 1].pose.position.y + current_path.poses[i + 1].pose.position.y - (2.0 * current_path.poses[i].pose.position.y)); + current_path.poses[i].pose.position.x += + weight_smooth * (current_path.poses[i - 1].pose.position.x + current_path.poses[i + 1].pose.position.x - + (2.0 * current_path.poses[i].pose.position.x)); + current_path.poses[i].pose.position.y += + weight_smooth * (current_path.poses[i - 1].pose.position.y + current_path.poses[i + 1].pose.position.y - + (2.0 * current_path.poses[i].pose.position.y)); change += fabs(xtemp - current_path.poses[i].pose.position.x); change += fabs(ytemp - current_path.poses[i].pose.position.y); @@ -239,25 +206,16 @@ bool smartcar::tools::Traj_Extractor::smooth_path(double weight_data, return true; } -double -smartcar::tools::Traj_Extractor::distance2points(const geometry_msgs::PoseStamped p1, - const geometry_msgs::PoseStamped p2) -{ - return std::sqrt(std::pow(p2.pose.position.x - p1.pose.position.x, 2) + - std::pow(p2.pose.position.y - p1.pose.position.y, 2)); +double smartcar::tools::Traj_Extractor::distance2points(const geometry_msgs::PoseStamped p1, const geometry_msgs::PoseStamped p2) { + return std::sqrt(std::pow(p2.pose.position.x - p1.pose.position.x, 2) + std::pow(p2.pose.position.y - p1.pose.position.y, 2)); } -bool smartcar::tools::Traj_Extractor::on_create_trajectory(traj_info &type) -{ - if (current_path.poses.empty()) - { +bool smartcar::tools::Traj_Extractor::on_create_trajectory(traj_info &type) { + if (current_path.poses.empty()) { ROS_WARN_STREAM("Current path unset! Please draw current path first."); return false; - } - else if (type.type == traj_type::LANE) - { - if (lane_list[type.id].path.poses.size() > 0) - { + } else if (type.type == traj_type::LANE) { + if (lane_list[type.id].path.poses.size() > 0) { lane_list[type.id].pre_ids.clear(); lane_list[type.id].next_ids.clear(); lane_list[type.id].path.poses.clear(); @@ -266,33 +224,27 @@ bool smartcar::tools::Traj_Extractor::on_create_trajectory(traj_info &type) // calculate the length of current_path double path_length = 0.0; - for (int i = 0; i < current_path.poses.size() - 1; i++) - { - path_length += - distance2points(current_path.poses[i], current_path.poses[i + 1]); + for (int i = 0; i < current_path.poses.size() - 1; i++) { + path_length += distance2points(current_path.poses[i], current_path.poses[i + 1]); } + type.length = path_length; lane_list[type.id].id = type.id; lane_list[type.id].reverse = type.reverse; lane_list[type.id].length = path_length; - for (int i = 0; i < type.pre_ids.size(); i++) - { + + for (int i = 0; i < type.pre_ids.size(); i++) { lane_list[type.id].pre_ids.push_back(type.pre_ids[i]); } - for (int i = 0; i < type.next_ids.size(); i++) - { + for (int i = 0; i < type.next_ids.size(); i++) { lane_list[type.id].next_ids.push_back(type.next_ids[i]); } lane_list[type.id].path = current_path; int size = lane_list[type.id].path.poses.size(); - std::cout << "Insert lane id: " << type.id << " with " << size << " points" - << std::endl; - } - else if (traj_type::CROSS) - { - if (cross_list[type.id].path.poses.size() > 0) - { + std::cout << "Insert lane id: " << type.id << " with " << size << " points" << std::endl; + } else if (traj_type::CROSS) { + if (cross_list[type.id].path.poses.size() > 0) { cross_list[type.id].pre_ids.clear(); cross_list[type.id].next_ids.clear(); cross_list[type.id].path.poses.clear(); @@ -301,60 +253,51 @@ bool smartcar::tools::Traj_Extractor::on_create_trajectory(traj_info &type) // calculate the length of current_path double path_length = 0.0; - for (int i = 0; i < current_path.poses.size() - 1; i++) - { - path_length += - distance2points(current_path.poses[i], current_path.poses[i + 1]); + for (int i = 0; i < current_path.poses.size() - 1; i++) { + path_length += distance2points(current_path.poses[i], current_path.poses[i + 1]); } type.length = path_length; cross_list[type.id].id = type.id; cross_list[type.id].reverse = type.reverse; cross_list[type.id].length = path_length; - for (int i = 0; i < type.pre_ids.size(); i++) - { + for (int i = 0; i < type.pre_ids.size(); i++) { cross_list[type.id].pre_ids.push_back(type.pre_ids[i]); } - for (int i = 0; i < type.next_ids.size(); i++) - { + for (int i = 0; i < type.next_ids.size(); i++) { cross_list[type.id].next_ids.push_back(type.next_ids[i]); } cross_list[type.id].path = current_path; int size = cross_list[type.id].path.poses.size(); - std::cout << "Insert cross id: " << type.id << " with " << size << " points" - << std::endl; + std::cout << "Insert cross id: " << type.id << " with " << size << " points" << std::endl; } add_text(type, current_path.poses[current_path.poses.size() / 2].pose); add_line(type); is_begin = true; show(); - current_path.poses.clear(); // 每次发布完后归位current_path + current_path.poses.clear(); // 每次发布完后归位current_path return true; } -void smartcar::tools::Traj_Extractor::reset_weight(double weight_data, double weight_smooth, double tolerance) -{ +void smartcar::tools::Traj_Extractor::reset_weight(double weight_data, double weight_smooth, double tolerance) { _weight_data = weight_data; _weight_smooth = weight_smooth; _tolerance = tolerance; on_weight_changed(); } -void smartcar::tools::Traj_Extractor::on_weight_changed() -{ +void smartcar::tools::Traj_Extractor::on_weight_changed() { smooth_path(_weight_data, _weight_smooth, _tolerance); pub_path.publish(current_path); } -bool smartcar::tools::Traj_Extractor::show() -{ +bool smartcar::tools::Traj_Extractor::show() { pub_markerArray.publish(lines); pub_texts.publish(texts); } -bool smartcar::tools::Traj_Extractor::add_text(const traj_info &type, const geometry_msgs::Pose &pose) -{ +bool smartcar::tools::Traj_Extractor::add_text(const traj_info &type, const geometry_msgs::Pose &pose) { visualization_msgs::Marker text; text.header.frame_id = "map"; text.header.stamp = ros::Time::now(); @@ -365,7 +308,7 @@ bool smartcar::tools::Traj_Extractor::add_text(const traj_info &type, const geom text.type = visualization_msgs::Marker::TEXT_VIEW_FACING; std::stringstream str; - if (type.type == traj_type::LANE) // lane -> blue // color range: [0,1] + if (type.type == traj_type::LANE) // lane -> blue // color range: [0,1] { text.scale.z = 1.0; text.color.g = 1; @@ -373,9 +316,7 @@ bool smartcar::tools::Traj_Extractor::add_text(const traj_info &type, const geom text.color.r = 0; text.color.a = 1; str << "lane " << type.id << "\n"; - } - else - { // cross -> red + } else { // cross -> red text.scale.z = 1.0; text.color.g = 0; text.color.b = 0; @@ -386,15 +327,13 @@ bool smartcar::tools::Traj_Extractor::add_text(const traj_info &type, const geom str << "length: " << type.length << "\n"; str << "pre_ids: "; - for (int i = 0; i < type.pre_ids.size(); i++) - { + for (int i = 0; i < type.pre_ids.size(); i++) { str << type.pre_ids[i] << " "; } str << "\n"; str << "next_ids: "; - for (int i = 0; i < type.next_ids.size(); i++) - { + for (int i = 0; i < type.next_ids.size(); i++) { str << type.next_ids[i] << " "; } str << "\n"; @@ -410,11 +349,9 @@ bool smartcar::tools::Traj_Extractor::add_text(const traj_info &type, const geom return true; } -bool smartcar::tools::Traj_Extractor::add_line(const traj_info &type) -{ +bool smartcar::tools::Traj_Extractor::add_line(const traj_info &type) { static int cnt = 1; - for (int j = 0; j < current_path.poses.size() - 1; j++) - { + for (int j = 0; j < current_path.poses.size() - 1; j++) { visualization_msgs::Marker line_strip, points; line_strip.header.frame_id = "map"; // points.header.frame_id = "map"; @@ -423,17 +360,14 @@ bool smartcar::tools::Traj_Extractor::add_line(const traj_info &type) // line_strip.ns = "WHOLE_PATH"; line_strip.action = visualization_msgs::Marker::ADD; - line_strip.id = cnt; // the id is unique. or you can use 'lifetime' to flush - // out the older id with newer one + line_strip.id = cnt; // the id is unique. or you can use 'lifetime' to flush + // out the older id with newer one line_strip.type = visualization_msgs::Marker::LINE_STRIP; - line_strip.scale.x = 0.2; // meters - if (type.type == traj_type::LANE) - { - line_strip.color.b = 1.0; // in the range of [0, 1] - line_strip.color.a = 1.0; // Don't forget to set the alpha! default:0 means transparent - } - else - { + line_strip.scale.x = 0.2; // meters + if (type.type == traj_type::LANE) { + line_strip.color.b = 1.0; // in the range of [0, 1] + line_strip.color.a = 1.0; // Don't forget to set the alpha! default:0 means transparent + } else { line_strip.color.r = 1.0; line_strip.color.a = 1.0; } @@ -453,56 +387,45 @@ bool smartcar::tools::Traj_Extractor::add_line(const traj_info &type) } } -bool smartcar::tools::Traj_Extractor::on_write_back() -{ +bool smartcar::tools::Traj_Extractor::on_write_back() { // int lane_size,cross_size; // for(lane_size = 0;!lane_list[lane_size].path.poses.empty();lane_size++); // for(cross_size = // 0;!cross_list[cross_size].path.poses.empty();cross_size++); - std::cout << "********* The Lane list is as followed *************" - << std::endl; - for (int i = 0; i < 101; i++) - { - if (lane_list[i].path.poses.empty()) - continue; + std::cout << "********* The Lane list is as followed *************" << std::endl; + for (int i = 0; i < 101; i++) { + if (lane_list[i].path.poses.empty()) continue; // pre std::cout << "lane id: " << i << std::endl; int before_size = lane_list[i].pre_ids.size(); std::cout << "(pre)before size: " << before_size << std::endl; - for (int j = 0; j < before_size; j++) - std::cout << lane_list[i].pre_ids[j] << " "; + for (int j = 0; j < before_size; j++) std::cout << lane_list[i].pre_ids[j] << " "; std::cout << std::endl; // next int before_size1 = lane_list[i].next_ids.size(); std::cout << "(next)before size: " << before_size1 << std::endl; - for (int j = 0; j < before_size1; j++) - std::cout << lane_list[i].next_ids[j] << " "; + for (int j = 0; j < before_size1; j++) std::cout << lane_list[i].next_ids[j] << " "; std::cout << std::endl; write_func("lane", i); std::cout << "-----------------" << std::endl; } - std::cout << "********* The Cross list is as followed *************" - << std::endl; - for (int i = 0; i < 101; i++) - { - if (cross_list[i].path.poses.empty()) - continue; + std::cout << "********* The Cross list is as followed *************" << std::endl; + for (int i = 0; i < 101; i++) { + if (cross_list[i].path.poses.empty()) continue; // pre std::cout << "cross id: " << i << std::endl; int before_size = cross_list[i].pre_ids.size(); std::cout << "(pre)before size: " << before_size << std::endl; - for (int j = 0; j < before_size; j++) - std::cout << cross_list[i].pre_ids[j] << " "; + for (int j = 0; j < before_size; j++) std::cout << cross_list[i].pre_ids[j] << " "; std::cout << std::endl; // next int before_size1 = cross_list[i].next_ids.size(); std::cout << "(next)before size: " << before_size1 << std::endl; - for (int j = 0; j < before_size1; j++) - std::cout << cross_list[i].next_ids[j] << " "; + for (int j = 0; j < before_size1; j++) std::cout << cross_list[i].next_ids[j] << " "; std::cout << std::endl; write_func("cross", i); @@ -511,57 +434,43 @@ bool smartcar::tools::Traj_Extractor::on_write_back() return true; } -bool smartcar::tools::Traj_Extractor::write_func(const std::string path_type, - const int id) -{ +bool smartcar::tools::Traj_Extractor::write_func(const std::string path_type, const int id) { std::stringstream file_name; file_name << _output_folder << path_type << "_" << id << ".csv"; std::ofstream f; f.open(file_name.str().c_str(), std::ios::out); - if (!f.is_open()) - { + if (!f.is_open()) { ROS_WARN_STREAM("Can not open file to save."); std::cout << "Check: " << file_name.str() << std::endl; return false; } // 由于Lane和Cross格式可能不一样,因此最好分别单独存储 - if (std::strcmp(path_type.c_str(), "lane") == 0) - { - f << id << std::endl - << lane_list[id].length << std::endl; - if (lane_list[id].reverse) - { + if (std::strcmp(path_type.c_str(), "lane") == 0) { + f << id << std::endl << lane_list[id].length << std::endl; + if (lane_list[id].reverse) { f << 1 << std::endl; - } - else - { + } else { f << 0 << std::endl; } - for (int i = 0; i < lane_list[id].pre_ids.size(); i++) - { + for (int i = 0; i < lane_list[id].pre_ids.size(); i++) { f << lane_list[id].pre_ids[i]; - if (i < lane_list[id].pre_ids.size() - 1) - f << ","; + if (i < lane_list[id].pre_ids.size() - 1) f << ","; } f << std::endl; - for (int i = 0; i < lane_list[id].next_ids.size(); i++) - { + for (int i = 0; i < lane_list[id].next_ids.size(); i++) { f << lane_list[id].next_ids[i]; - if (i < lane_list[id].next_ids.size() - 1) - f << ","; + if (i < lane_list[id].next_ids.size() - 1) f << ","; } f << std::endl; int len_lane = lane_list[id].path.poses.size(); - for (size_t i = 0; i < len_lane - 1; i++) - { + for (size_t i = 0; i < len_lane - 1; i++) { geometry_msgs::PoseStamped p_c = lane_list[id].path.poses[i]; geometry_msgs::PoseStamped p_n = lane_list[id].path.poses[i + 1]; - double yaw = std::atan2(p_n.pose.position.y - p_c.pose.position.y, - p_n.pose.position.x - p_c.pose.position.x); + double yaw = std::atan2(p_n.pose.position.y - p_c.pose.position.y, p_n.pose.position.x - p_c.pose.position.x); Eigen::AngleAxisd rollangle(0, Eigen::Vector3d::UnitX()); Eigen::AngleAxisd yawangle(yaw, Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchangle(0, Eigen::Vector3d::UnitY()); @@ -571,62 +480,42 @@ bool smartcar::tools::Traj_Extractor::write_func(const std::string path_type, lane_list[id].path.poses[i].pose.orientation.z = q.z(); lane_list[id].path.poses[i].pose.orientation.w = q.w(); } - lane_list[id].path.poses[len_lane - 1].pose.orientation.x = - lane_list[id].path.poses[len_lane - 2].pose.orientation.x; - lane_list[id].path.poses[len_lane - 1].pose.orientation.y = - lane_list[id].path.poses[len_lane - 2].pose.orientation.y; - lane_list[id].path.poses[len_lane - 1].pose.orientation.z = - lane_list[id].path.poses[len_lane - 2].pose.orientation.z; - lane_list[id].path.poses[len_lane - 1].pose.orientation.w = - lane_list[id].path.poses[len_lane - 2].pose.orientation.w; - - for (int i = 0; i < lane_list[id].path.poses.size(); i++) - { - f << lane_list[id].path.poses[i].pose.position.x << "," - << lane_list[id].path.poses[i].pose.position.y << "," - << lane_list[id].path.poses[i].pose.position.z << "," - << lane_list[id].path.poses[i].pose.orientation.x << "," - << lane_list[id].path.poses[i].pose.orientation.y << "," - << lane_list[id].path.poses[i].pose.orientation.z << "," + lane_list[id].path.poses[len_lane - 1].pose.orientation.x = lane_list[id].path.poses[len_lane - 2].pose.orientation.x; + lane_list[id].path.poses[len_lane - 1].pose.orientation.y = lane_list[id].path.poses[len_lane - 2].pose.orientation.y; + lane_list[id].path.poses[len_lane - 1].pose.orientation.z = lane_list[id].path.poses[len_lane - 2].pose.orientation.z; + lane_list[id].path.poses[len_lane - 1].pose.orientation.w = lane_list[id].path.poses[len_lane - 2].pose.orientation.w; + + for (int i = 0; i < lane_list[id].path.poses.size(); i++) { + f << lane_list[id].path.poses[i].pose.position.x << "," << lane_list[id].path.poses[i].pose.position.y << "," + << lane_list[id].path.poses[i].pose.position.z << "," << lane_list[id].path.poses[i].pose.orientation.x << "," + << lane_list[id].path.poses[i].pose.orientation.y << "," << lane_list[id].path.poses[i].pose.orientation.z << "," << lane_list[id].path.poses[i].pose.orientation.w << std::endl; } - } - else - { - f << id << std::endl - << cross_list[id].length << std::endl; - if (cross_list[id].reverse) - { + } else { + f << id << std::endl << cross_list[id].length << std::endl; + if (cross_list[id].reverse) { f << 1 << std::endl; - } - else - { + } else { f << 0 << std::endl; } - for (int i = 0; i < cross_list[id].pre_ids.size(); i++) - { + for (int i = 0; i < cross_list[id].pre_ids.size(); i++) { f << cross_list[id].pre_ids[i]; - if (i < cross_list[id].pre_ids.size() - 1) - f << ","; + if (i < cross_list[id].pre_ids.size() - 1) f << ","; } f << std::endl; - for (int i = 0; i < cross_list[id].next_ids.size(); i++) - { + for (int i = 0; i < cross_list[id].next_ids.size(); i++) { f << cross_list[id].next_ids[i]; - if (i < cross_list[id].next_ids.size() - 1) - f << ","; + if (i < cross_list[id].next_ids.size() - 1) f << ","; } f << std::endl; int len_lane = cross_list[id].path.poses.size(); - for (size_t i = 0; i < len_lane - 1; i++) - { + for (size_t i = 0; i < len_lane - 1; i++) { geometry_msgs::PoseStamped p_c = cross_list[id].path.poses[i]; geometry_msgs::PoseStamped p_n = cross_list[id].path.poses[i + 1]; - double yaw = std::atan2(p_n.pose.position.y - p_c.pose.position.y, - p_n.pose.position.x - p_c.pose.position.x); + double yaw = std::atan2(p_n.pose.position.y - p_c.pose.position.y, p_n.pose.position.x - p_c.pose.position.x); Eigen::AngleAxisd rollangle(0, Eigen::Vector3d::UnitX()); Eigen::AngleAxisd yawangle(yaw, Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchangle(0, Eigen::Vector3d::UnitY()); @@ -636,23 +525,15 @@ bool smartcar::tools::Traj_Extractor::write_func(const std::string path_type, cross_list[id].path.poses[i].pose.orientation.z = q.z(); cross_list[id].path.poses[i].pose.orientation.w = q.w(); } - cross_list[id].path.poses[len_lane - 1].pose.orientation.x = - cross_list[id].path.poses[len_lane - 2].pose.orientation.x; - cross_list[id].path.poses[len_lane - 1].pose.orientation.y = - cross_list[id].path.poses[len_lane - 2].pose.orientation.y; - cross_list[id].path.poses[len_lane - 1].pose.orientation.z = - cross_list[id].path.poses[len_lane - 2].pose.orientation.z; - cross_list[id].path.poses[len_lane - 1].pose.orientation.w = - cross_list[id].path.poses[len_lane - 2].pose.orientation.w; - - for (int i = 0; i < cross_list[id].path.poses.size(); i++) - { - f << cross_list[id].path.poses[i].pose.position.x << "," - << cross_list[id].path.poses[i].pose.position.y << "," - << cross_list[id].path.poses[i].pose.position.z << "," - << cross_list[id].path.poses[i].pose.orientation.x << "," - << cross_list[id].path.poses[i].pose.orientation.y << "," - << cross_list[id].path.poses[i].pose.orientation.z << "," + cross_list[id].path.poses[len_lane - 1].pose.orientation.x = cross_list[id].path.poses[len_lane - 2].pose.orientation.x; + cross_list[id].path.poses[len_lane - 1].pose.orientation.y = cross_list[id].path.poses[len_lane - 2].pose.orientation.y; + cross_list[id].path.poses[len_lane - 1].pose.orientation.z = cross_list[id].path.poses[len_lane - 2].pose.orientation.z; + cross_list[id].path.poses[len_lane - 1].pose.orientation.w = cross_list[id].path.poses[len_lane - 2].pose.orientation.w; + + for (int i = 0; i < cross_list[id].path.poses.size(); i++) { + f << cross_list[id].path.poses[i].pose.position.x << "," << cross_list[id].path.poses[i].pose.position.y << "," + << cross_list[id].path.poses[i].pose.position.z << "," << cross_list[id].path.poses[i].pose.orientation.x << "," + << cross_list[id].path.poses[i].pose.orientation.y << "," << cross_list[id].path.poses[i].pose.orientation.z << "," << cross_list[id].path.poses[i].pose.orientation.w << std::endl; } } @@ -661,13 +542,9 @@ bool smartcar::tools::Traj_Extractor::write_func(const std::string path_type, return true; } -bool smartcar::tools::Traj_Extractor::shutdown() -{ - return on_shutdown(); -} +bool smartcar::tools::Traj_Extractor::shutdown() { return on_shutdown(); } -bool smartcar::tools::Traj_Extractor::on_shutdown() -{ +bool smartcar::tools::Traj_Extractor::on_shutdown() { ros::shutdown(); return true; } \ No newline at end of file