Merge pull request #1190 from usdot-fhwa-stol/hotfix/candidate_3.5.3

Hotfix/candidate 3.5.3

carma/launch/environment.launch

@@ -46,13 +46,14 @@
 
   <!-- CARMA Motion Computation Node -->
   <group>
-    <remap to="$(optenv CARMA_MSG_NS)/incoming_mobility_path" from="/environment/incoming_mobility_path"/>
+    <remap to="$(optenv CARMA_MSG_NS)/incoming_mobility_path" from="incoming_mobility_path"/>
     <include file="$(find motion_computation)/launch/motion_computation.launch"/>
   </group>
 
 
   <!-- CARMA External Object Node -->
   <group unless="$(arg simulation_mode)">
+    <remap to="prediction/motion_predictor/objects" from="detected_objects"/>
     <include file="$(find object_detection_tracking)/launch/external_object.launch"/>
   </group>
 
@@ -82,6 +83,7 @@
 
   <!-- vision_darknet_detect -->
   <group>
+  <remap from="/detection/image_detector/objects" to="detection/image_detector/objects"/>
   <include file="$(find vision_darknet_detect)/launch/vision_yolo3_detect.launch">
   <arg name="gpu_device_id" value="0"/>
   <arg name="score_threshold" value="0.30"/>
@@ -97,6 +99,8 @@
   </group>
 
 <!-- vision_beyond_track -->
+  <remap to="detection/image_detector/objects" from="vision_beyond_track_01/detection/image_detector/objects"/>
+  <remap from="/detection/image_tracker/objects" to="detection/image_tracker/objects"/>
   <group>
   <include file="$(find vision_beyond_track)/launch/vision_beyond_track.launch">
   <arg name="camera_info_src" value="$(optenv CARMA_INTR_NS)/camera/camera_info"/>
@@ -106,7 +110,12 @@
   </group>
 
   <!-- lidar_euclidean_cluster_detect -->
+  <!-- todo: remove CARMA_ENV_NS namespace without the node generating extra unwanted topics-->
   <group>
+  <remap to="$(optenv CARMA_ENV_NS)/detection/lidar_detector/cloud_clusters" from="/detection/lidar_detector/cloud_clusters"/>
+  <remap to="$(optenv CARMA_ENV_NS)/detection/lidar_detector/objects" from="/detection/lidar_detector/objects"/>
+  <remap to="$(optenv CARMA_ENV_NS)/detection/lidar_detector/objects_markers" from="/detection/lidar_detector/objects_markers"/>
+
   <include file="$(find lidar_euclidean_cluster_detect)/launch/lidar_euclidean_cluster_detect.launch">
   <arg name="points_node" value="ray_ground_filter/points_no_ground" />
 
@@ -139,9 +148,10 @@
   <!-- range_vision_fusion -->
   <group>
 
-  <remap to="/detection/image_detector/objects" from="/environment/range_vision_fusion_01/detection/image_detector/objects"/>
-  <remap to="/detection/lidar_detector/objects" from="/environment/range_vision_fusion_01/detection/lidar_detector/objects"/>
-
+  <remap to="detection/image_tracker/objects" from="range_vision_fusion_01/detection/image_detector/objects"/>
+  <remap to="detection/lidar_detector/objects" from="range_vision_fusion_01/detection/lidar_detector/objects"/>
+  <remap to="detection/fusion_tools/objects" from="/detection/fusion_tools/objects"/>
+
   <include file="$(find range_vision_fusion)/launch/range_vision_fusion.launch">
   <arg name="detected_objects_range" value="detection/lidar_detector/objects"/>
   <arg name="detected_objects_vision" value="detection/image_detector/objects"/>
@@ -159,10 +169,12 @@
 
   <!-- imm_ukf_pda_track -->
   <group>
+  <remap to="detected_objects" from="/detection/objects"/>
+
   <include file="$(find imm_ukf_pda_track)/launch/imm_ukf_pda_track_lanelet2.launch">
-  	 <arg name="namespace" value="detection/object_tracker"/>
+  <arg name="namespace" value="detection/object_tracker"/>
   <arg name="tracker_input_topic" value="detection/fusion_tools/objects" />
-  	<arg name="tracker_output_topic" value="detection/object_tracker/objects" />
+  <arg name="tracker_output_topic" value="detection/object_tracker/objects" />
 
   <arg name="tracking_frame" value="map" />
   <arg name="gating_threshold" value="9.22" />
@@ -185,14 +197,17 @@
   </group>
 
   <!-- naive_motion_predict -->
+  <!-- todo: remove CARMA_ENV_NS namespace without the node generating extra unwanted topics-->
   <group>
-  <remap from="/prediction/motion_predictor/objects" to="detected_objects"/>
+  <remap to="$(optenv CARMA_ENV_NS)/prediction/motion_predictor/objects" from="/prediction/motion_predictor/objects"/>
+  <remap to="$(optenv CARMA_ENV_NS)/prediction/motion_predictor/path_markers" from="/prediction/motion_predictor/path_markers"/>
+  <remap to="$(optenv CARMA_ENV_NS)/prediction/motion_predictor/objects_markers" from="/prediction/motion_predictor/objects_markers"/>
   <include file="$(find naive_motion_predict)/launch/naive_motion_predict.launch">
   <arg name="interval_sec" value="0.1"/>
   <arg name="num_prediction" value="10"/>
   <arg name="sensor_height_calibration_params_file" value="$(arg vehicle_calibration_dir)/naive_motion_predict/calibration.yaml"/>
   <arg name="filter_out_close_object_threshold" value="1.5"/>
-  <arg name="input_topic" value="/detection/fusion_tools/objects"/>
+  <arg name="input_topic" value="detected_objects"/>
   </include>
   </group>
 
@@ -205,4 +220,4 @@
     </include>
   </group>
 
-</launch>
+</launch>

carma/rviz/carma_default.rviz

@@ -557,12 +557,12 @@ Visualization Manager:
       width: 128
     - Class: rviz/MarkerArray
       Enabled: true
-      Marker Topic: /detection/lidar_detector/objects_markers
+      Marker Topic: /environment/detection/lidar_detector/objects_markers
       Name: MarkerArray
       Namespaces:
-        /detection/lidar_detector/centroid_markers: true
-        /detection/lidar_detector/hull_markers: true
-        /detection/lidar_detector/label_markers: true
+        /environment/detection/lidar_detector/centroid_markers: true
+        /environment/detection/lidar_detector/hull_markers: true
+        /environment/detection/lidar_detector/label_markers: true
       Queue Size: 100
       Value: true
     - Alpha: 1

object_detection_tracking/src/object_detection_tracking_worker.cpp

@@ -103,7 +103,7 @@ void ObjectDetectionTrackingWorker::detectedObjectCallback(const autoware_msgs::
     {
       obj.object_type = obj.LARGE_VEHICLE;
     }
-    else if (obj_array.objects[i].label.compare("person"))
+    else if (obj_array.objects[i].label.compare("person") == 0)
     {
       obj.object_type = obj.PEDESTRIAN;
     }

route_following_plugin/src/route_following_plugin.cpp

@@ -46,7 +46,7 @@ namespace route_following_plugin
 
         pnh_->param<double>("minimal_maneuver_duration", mvr_duration_, 16.0);
         pnh2_->param<double>("config_speed_limit",config_limit);
-        pnh_->param<double>("/guidance/route_end_jerk", jerk_, 1.0);
+        pnh_->param<double>("/guidance/route_end_jerk", jerk_, 0.05);
         wml_.reset(new carma_wm::WMListener());
         // set world model point form wm listener
         wm_ = wml_->getWorldModel();

stop_and_wait_plugin/src/stop_and_wait_plugin.cpp

@@ -129,12 +129,8 @@ namespace stop_and_wait_plugin
             return true;
         }
 
-        // Update state to correctly reflect current pos
-        auto curr_state = req.vehicle_state;
-        curr_state.X_pos_global = pose_msg_.pose.position.x;
-        curr_state.Y_pos_global = pose_msg_.pose.position.y;
 
-        std::vector<PointSpeedPair> points_and_target_speeds = maneuvers_to_points(maneuver_plan, current_downtrack, wm_, curr_state);
+        std::vector<PointSpeedPair> points_and_target_speeds = maneuvers_to_points(maneuver_plan, current_downtrack, wm_, req.vehicle_state);
 
         auto downsampled_points = 
             carma_utils::containers::downsample_vector(points_and_target_speeds,downsample_ratio_);
@@ -145,7 +141,7 @@ namespace stop_and_wait_plugin
         trajectory.header.stamp = ros::Time::now();
         trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
 
-        trajectory.trajectory_points = compose_trajectory_from_centerline(downsampled_points,curr_state);
+        trajectory.trajectory_points = compose_trajectory_from_centerline(downsampled_points,req.vehicle_state);
         ROS_DEBUG_STREAM("Trajectory points size:"<<trajectory.trajectory_points.size());
         trajectory.initial_longitudinal_velocity = req.vehicle_state.longitudinal_vel;
         resp.trajectory_plan = trajectory;
@@ -254,11 +250,16 @@ namespace stop_and_wait_plugin
 
                 lanelet::BasicLineString2d route_geometry = carma_wm::geometry::concatenate_lanelets(lanelets_to_add);
                 int nearest_pt_index = getNearestRouteIndex(route_geometry,state);
+                // route end point index
                 auto temp_state = state;
-                temp_state.X_pos_global = wm_->getRoute()->getEndPoint().basicPoint2d().x();
-                temp_state.Y_pos_global =  wm_->getRoute()->getEndPoint().basicPoint2d().y();
-                int nearest_end_pt_index = getNearestRouteIndex(route_geometry,temp_state);
-                lanelet::BasicLineString2d future_route_geometry(route_geometry.begin() + nearest_pt_index, route_geometry.begin()+ nearest_end_pt_index);
+
+                // maneuver end dist index
+                int ending_downtrack_pt_index = (int)(route_geometry.size() * (ending_downtrack / wm_->getRoute()->length2d()));
+                ROS_DEBUG_STREAM("ending_downtrack: " << ending_downtrack);
+                ROS_DEBUG_STREAM("ending_downtrack_pt_index" << ending_downtrack_pt_index);
+                ROS_DEBUG_STREAM("nearest_pt_index" << nearest_pt_index);
+
+                lanelet::BasicLineString2d future_route_geometry(route_geometry.begin() + nearest_pt_index, route_geometry.begin()+ ending_downtrack_pt_index);
 
                 int points_count = future_route_geometry.size();
                 delta_time = maneuver_time_/(points_count-1);

trajectory_executor/include/trajectory_executor/trajectory_executor.hpp

@@ -30,14 +30,6 @@
 #include <ros/callback_queue.h>
 
 namespace trajectory_executor {
-    /*!
-     * \brief This method trims the first point off of a TrajectoryPlan's point
-     * and returns a new message with the update.
-     * 
-     * \param plan The plan to modify
-     * \return A new message with the copied contents minus the first point
-     */
-    cav_msgs::TrajectoryPlan trimPastPoints(const cav_msgs::TrajectoryPlan &plan);
 
     /**
      * Trajectory Executor package primary worker class

trajectory_executor/src/trajectory_executor/trajectory_executor.cpp

@@ -22,20 +22,6 @@
 
 namespace trajectory_executor 
 {
-    cav_msgs::TrajectoryPlan trimPastPoints(const cav_msgs::TrajectoryPlan &plan) {
-        cav_msgs::TrajectoryPlan out(plan);
-        out.trajectory_points = std::vector<cav_msgs::TrajectoryPlanPoint>();
-
-        ros::Time current_time = ros::Time::now();
-
-        for (int i = 0; i < plan.trajectory_points.size(); i++) {
-            if (plan.trajectory_points[i].target_time > current_time) {
-                out.trajectory_points.push_back(plan.trajectory_points[i]);
-            }
-        }
-
-        return out;
-    }
 
     TrajectoryExecutor::TrajectoryExecutor(int traj_frequency) :
         _timesteps_since_last_traj(0),
@@ -93,9 +79,6 @@ namespace trajectory_executor
         ROS_DEBUG("TrajectoryExecutor tick start!");
 
         if (_cur_traj != nullptr) {
-            if (_timesteps_since_last_traj > 0) {
-                _cur_traj = std::unique_ptr<cav_msgs::TrajectoryPlan>(new cav_msgs::TrajectoryPlan(trimPastPoints(*_cur_traj)));
-            }
             if (!_cur_traj->trajectory_points.empty()) {
                 // Determine the relevant control plugin for the current timestep
                 std::string control_plugin = _cur_traj->trajectory_points[0].controller_plugin_name;