From 29de47c7bebf1edca680cf67fca23af3ab085dec Mon Sep 17 00:00:00 2001 From: Kenji Funaoka Date: Fri, 28 Dec 2018 12:48:37 +0900 Subject: [PATCH] Cleanup WaypointState.msg and add sttering_state=STR_BACK --- .../decision_maker_node_callback.cpp | 10 ++++---- .../decision_maker_node_stateupdate.cpp | 6 ++--- .../waypoint_loader/waypoint_loader_core.cpp | 4 ++-- .../msgs/autoware_msgs/msg/WaypointState.msg | 23 ++++++++++++++----- 4 files changed, 27 insertions(+), 16 deletions(-) diff --git a/ros/src/computing/planning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_callback.cpp b/ros/src/computing/planning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_callback.cpp index 92ebe2f4b86..dcbeebf49da 100644 --- a/ros/src/computing/planning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_callback.cpp +++ b/ros/src/computing/planning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_callback.cpp @@ -284,8 +284,8 @@ void DecisionMakerNode::setWaypointState(autoware_msgs::LaneArray& lane_array) lane.waypoints.at(wp_idx).pose.pose.position.y, lane.waypoints.at(wp_idx + 1).pose.pose.position.x, lane.waypoints.at(wp_idx + 1).pose.pose.position.y)) { - lane.waypoints.at(wp_idx).wpstate.stopline_state = g_vmap.findByKey(Key(stopline.signid)).type; - // lane.waypoints.at(wp_idx + 1).wpstate.stopline_state = 1; + lane.waypoints.at(wp_idx).wpstate.stop_state = g_vmap.findByKey(Key(stopline.signid)).type; + // lane.waypoints.at(wp_idx + 1).wpstate.stop_state = 1; } } } @@ -311,7 +311,7 @@ void DecisionMakerNode::callbackFromLaneWaypoint(const autoware_msgs::LaneArray wp.wpstate.aid = 0; wp.wpstate.steering_state = autoware_msgs::WaypointState::NULLSTATE; wp.wpstate.accel_state = autoware_msgs::WaypointState::NULLSTATE; - wp.wpstate.stopline_state = autoware_msgs::WaypointState::NULLSTATE; + wp.wpstate.stop_state = autoware_msgs::WaypointState::NULLSTATE; wp.wpstate.lanechange_state = autoware_msgs::WaypointState::NULLSTATE; wp.wpstate.event_state = 0; } @@ -361,12 +361,12 @@ void DecisionMakerNode::callbackFromFinalWaypoint(const autoware_msgs::Lane &msg { if (i < current_finalwaypoints_.waypoints.size()) { - if (current_finalwaypoints_.waypoints.at(i).wpstate.stopline_state == autoware_msgs::WaypointState::TYPE_STOPLINE) + if (current_finalwaypoints_.waypoints.at(i).wpstate.stop_state == autoware_msgs::WaypointState::TYPE_STOPLINE) { ctx->setCurrentState(state_machine::DRIVE_ACC_STOPLINE_STATE); closest_stopline_waypoint_ = CurrentStoplineTarget_.gid; } - if (current_finalwaypoints_.waypoints.at(i).wpstate.stopline_state == autoware_msgs::WaypointState::TYPE_STOP) + if (current_finalwaypoints_.waypoints.at(i).wpstate.stop_state == autoware_msgs::WaypointState::TYPE_STOP) ctx->setCurrentState(state_machine::DRIVE_ACC_STOP_STATE); } } diff --git a/ros/src/computing/planning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_stateupdate.cpp b/ros/src/computing/planning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_stateupdate.cpp index 65a92d18450..e189c49ff4a 100644 --- a/ros/src/computing/planning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_stateupdate.cpp +++ b/ros/src/computing/planning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_stateupdate.cpp @@ -196,7 +196,7 @@ void DecisionMakerNode::updateLaneWaypointsArray(void) for (auto& wp : lane.waypoints) { wp.twist.twist.linear.x = 0.0; - wp.wpstate.stopline_state = 0; + wp.wpstate.stop_state = 0; } } for (auto& lane : current_shifted_lane_array_.lanes) @@ -206,7 +206,7 @@ void DecisionMakerNode::updateLaneWaypointsArray(void) // if stopped at stopline, to delete flags already used. if (CurrentStoplineTarget_.gid - 2 <= wp.gid && wp.gid <= CurrentStoplineTarget_.gid + 2) { - wp.wpstate.stopline_state = 0; + wp.wpstate.stop_state = 0; } } } @@ -262,7 +262,7 @@ void DecisionMakerNode::setAllStoplineStop(void) b->x = lane.waypoints.at(wp_idx).pose.pose.position.x; b->y = lane.waypoints.at(wp_idx).pose.pose.position.y; if (amathutils::find_distance(a, b) <= 4) // - lane.waypoints.at(wp_idx).wpstate.stopline_state = 1; + lane.waypoints.at(wp_idx).wpstate.stop_state = 1; } } } diff --git a/ros/src/computing/planning/motion/packages/waypoint_maker/nodes/waypoint_loader/waypoint_loader_core.cpp b/ros/src/computing/planning/motion/packages/waypoint_maker/nodes/waypoint_loader/waypoint_loader_core.cpp index 36c307be33b..9dc32c47a42 100644 --- a/ros/src/computing/planning/motion/packages/waypoint_maker/nodes/waypoint_loader/waypoint_loader_core.cpp +++ b/ros/src/computing/planning/motion/packages/waypoint_maker/nodes/waypoint_loader/waypoint_loader_core.cpp @@ -139,7 +139,7 @@ void WaypointLoaderNode::saveLaneArray(const std::vector& paths, ofs << std::fixed << std::setprecision(4) << el.pose.pose.position.x << "," << el.pose.pose.position.y << "," << el.pose.pose.position.z << "," << tf::getYaw(el.pose.pose.orientation) << "," << mps2kmph(el.twist.twist.linear.x) << "," << (int)el.change_flag << "," << (int)el.wpstate.steering_state - << "," << (int)el.wpstate.accel_state << "," << (int)el.wpstate.stopline_state << "," + << "," << (int)el.wpstate.accel_state << "," << (int)el.wpstate.stop_state << "," << (int)el.wpstate.event_state << std::endl; } idx++; @@ -294,7 +294,7 @@ void WaypointLoaderNode::parseWaypointForVer3(const std::string& line, const std wp->change_flag = std::stoi(map["change_flag"]); wp->wpstate.steering_state = (map.find("steering_flag") != map.end()) ? std::stoi(map["steering_flag"]) : 0; wp->wpstate.accel_state = (map.find("accel_flag") != map.end()) ? std::stoi(map["accel_flag"]) : 0; - wp->wpstate.stopline_state = (map.find("stop_flag") != map.end()) ? std::stoi(map["stop_flag"]) : 0; + wp->wpstate.stop_state = (map.find("stop_flag") != map.end()) ? std::stoi(map["stop_flag"]) : 0; wp->wpstate.event_state = (map.find("event_flag") != map.end()) ? std::stoi(map["event_flag"]) : 0; } diff --git a/ros/src/msgs/autoware_msgs/msg/WaypointState.msg b/ros/src/msgs/autoware_msgs/msg/WaypointState.msg index c2df3ac9995..4486b77a75d 100644 --- a/ros/src/msgs/autoware_msgs/msg/WaypointState.msg +++ b/ros/src/msgs/autoware_msgs/msg/WaypointState.msg @@ -1,16 +1,27 @@ int32 aid uint8 NULLSTATE=0 + +# lanechange uint8 lanechange_state +# bilinker uint8 steering_state uint8 STR_LEFT=1 uint8 STR_RIGHT=2 uint8 STR_STRAIGHT=3 +uint8 STR_BACK=4 uint8 accel_state -uint8 stopline_state - uint8 TYPE_NULL=0 - uint8 TYPE_STOPLINE=1 - uint8 TYPE_STOP=2 -# 1 is stopline, 2 is stop which 2 can only be released manually. -uint64 event_state + +uint8 stop_state +# 1 is stopline, 2 is stop which can only be released manually. +uint8 TYPE_STOPLINE=1 +uint8 TYPE_STOP=2 + +uint8 event_state +uint8 TYPE_EVENT_NULL = 0 +uint8 TYPE_EVENT_GOAL = 1 +uint8 TYPE_EVENT_MIDDLE_GOAL = 2 +uint8 TYPE_EVENT_POSITION_STOP = 3 +uint8 TYPE_EVENT_BUS_STOP = 4 +uint8 TYPE_EVENT_PARKING = 5