Method FissPlannerNode

src/fiss_planner_node.cpp:112–171

Constructor

// Constructor
FissPlannerNode::FissPlannerNode() : tf_listener(tf_buffer)
{
  // topics
  std::string odom_topic;
  std::string lane_info_topic;
  std::string obstacles_topic;

  std::string ref_path_topic;
  std::string curr_traj_topic;
  std::string next_traj_topic;
  std::string sample_space_topic;
  std::string final_traj_topic;
  std::string candidate_trajs_topic;
  std::string vehicle_cmd_topic;

  ros::NodeHandle private_nh("~");
  f = boost::bind(&dynamicParamCallback, _1, _2);
  server.setCallback(f);

  // Hyperparameters from launch file
  ROS_ASSERT(private_nh.getParam("odom_topic", odom_topic));
  ROS_ASSERT(private_nh.getParam("lane_info_topic", lane_info_topic));
  ROS_ASSERT(private_nh.getParam("obstacles_topic", obstacles_topic));
  ROS_ASSERT(private_nh.getParam("ref_path_topic", ref_path_topic));
  ROS_ASSERT(private_nh.getParam("curr_traj_topic", curr_traj_topic));
  ROS_ASSERT(private_nh.getParam("next_traj_topic", next_traj_topic));
  ROS_ASSERT(private_nh.getParam("sample_space_topic", sample_space_topic));
  ROS_ASSERT(private_nh.getParam("final_traj_topic", final_traj_topic));
  ROS_ASSERT(private_nh.getParam("candidate_trajs_topic", candidate_trajs_topic));
  ROS_ASSERT(private_nh.getParam("vehicle_cmd_topic", vehicle_cmd_topic));

  // Instantiate FissPlanner
  frenet_planner_ = FissPlanner(SETTINGS);

  // Subscribe & Advertise
  odom_sub = nh.subscribe(odom_topic, 1, &FissPlannerNode::odomCallback, this);
  lane_info_sub = nh.subscribe(lane_info_topic, 1, &FissPlannerNode::laneInfoCallback, this);
  obstacles_sub = nh.subscribe(obstacles_topic, 1, &FissPlannerNode::obstaclesCallback, this);
  
  ref_path_pub = nh.advertise<nav_msgs::Path>(ref_path_topic, 1);
  curr_traj_pub = nh.advertise<nav_msgs::Path>(curr_traj_topic, 1);
  next_traj_pub = nh.advertise<nav_msgs::Path>(next_traj_topic, 1);

  sample_space_pub = nh.advertise<visualization_msgs::Marker>(sample_space_topic, 1);
  final_traj_pub = nh.advertise<visualization_msgs::Marker>(final_traj_topic, 1);
  candidate_paths_pub = nh.advertise<visualization_msgs::MarkerArray>(candidate_trajs_topic, 1);

  vehicle_cmd_pub = nh.advertise<autoware_msgs::VehicleCmd>(vehicle_cmd_topic, 1);
  obstacles_pub = nh.advertise<autoware_msgs::DetectedObjectArray>("fiss_planner/objects", 1);

  run_time_pub = nh.advertise<std_msgs::Float32>("fiss_planner/run_time", 1);
  freqency_pub = nh.advertise<std_msgs::Float32>("fiss_planner/freqency", 1);
  iteration_pub = nh.advertise<std_msgs::Float32>("fiss_planner/iteration", 1);
  cost_pub = nh.advertise<std_msgs::Float32>("fiss_planner/cost", 1);

  // Initializing states
  regenerate_flag_ = false;
  target_lane_id_ = LaneID::CURR_LANE;