Merge pull request #46 from Ashuh/organize-launch

Organize launch

collision_warning_system/include/collision_warning_system/bicycle_model.h

@@ -9,10 +9,6 @@
 
 #include "collision_warning_system/bicycle_state.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 class BicycleModel
 {
 private:
@@ -29,7 +25,5 @@ class BicycleModel
                                    const double velocity, const double steering_angle, const double delta_t,
                                    const double t_max);
 };
-}  // namespace safety
-}  // namespace f1tenth_racecar
 
 #endif  // COLLISION_WARNING_SYSTEM_BICYCLE_MODEL_H

collision_warning_system/include/collision_warning_system/bicycle_state.h

@@ -1,10 +1,6 @@
 #ifndef COLLISION_WARNING_SYSTEM_BICYCLE_STATE_H
 #define COLLISION_WARNING_SYSTEM_BICYCLE_STATE_H
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 class BicycleState
 {
 private:
@@ -24,7 +20,4 @@ class BicycleState
   double steering_angle() const;
 };
 
-}  // namespace safety
-}  // namespace f1tenth_racecar
-
 #endif  // COLLISION_WARNING_SYSTEM_BICYCLE_STATE_H

collision_warning_system/include/collision_warning_system/collision_warning_system.h

@@ -14,10 +14,6 @@
 #include "collision_warning_system/bicycle_state.h"
 #include "costmap_generator/collision_checker.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 class CollisionWarningSystem
 {
 public:
@@ -58,6 +54,5 @@ class CollisionWarningSystem
 
   void visualizeProjectedTrajectory(const nav_msgs::Path& path);
 };
-}  // namespace safety
-}  // namespace f1tenth_racecar
+
 #endif  // COLLISION_WARNING_SYSTEM_COLLISION_WARNING_SYSTEM_H

collision_warning_system/launch/collision_warning_system.launch

@@ -1,21 +1,16 @@
 <launch>
-    <arg name="circle_offsets" value="[0.1, 0.3]" />
-    <arg name="circle_radius" value="0.2" />
-    <arg name="t_max" value="2"/>
-    <arg name="delta_t" value="0.1"/>
+    <arg name="circle_offsets" default="[0.1, 0.3]" />
+    <arg name="circle_radius" default="0.2" />
+    <arg name="t_max" default="2"/>
+    <arg name="delta_t" default="0.1"/>
 
-    <arg name="wheelbase" value="0.3"/>
-    <arg name="vehicle_width" value="0.3"/>
-    <arg name="vehicle_length" value="0.5"/>
-    <arg name="base_link_to_center_dist" value="0.1"/>
+    <arg name="wheelbase" default="0.3"/>
+    <arg name="vehicle_width" default="0.3"/>
+    <arg name="vehicle_length" default="0.5"/>
+    <arg name="base_link_to_center_dist" default="0.1"/>
 
-    <arg name="odom_topic" value="odom"/>
-    <arg name="drive_topic" value="drive"/>
-    <arg name="obstacle_topic" value="obstacles"/>
-    <arg name="time_to_collision_topic" value="time_to_collision"/>
-    <arg name="trajectory_topic" value="trajectory"/>
-    <arg name="vehicle_footprints_viz_topic" value="vehicle_footprints_viz"/>
-    <arg name="collision_viz_topic" value="collision_viz"/>
+    <arg name="odom_topic" default="odom"/>
+    <arg name="drive_topic" default="drive"/>
 
     <node name="collision_warning_system" pkg="collision_warning_system" type="collision_warning_system_node" output="screen">
         <rosparam param="circle_offsets" subst_value="True">$(arg circle_offsets)</rosparam>
@@ -28,12 +23,7 @@
         <param name="vehicle_length" value="$(arg vehicle_length)"/>
         <param name="base_link_to_center_dist" value="$(arg base_link_to_center_dist)"/>
 
-        <param name="odom_topic" value="$(arg odom_topic)"/>
-        <param name="drive_topic" value="$(arg drive_topic)"/>
-        <param name="obstacle_topic" value="$(arg obstacle_topic)"/>
-        <param name="time_to_collision_topic" value="$(arg time_to_collision_topic)"/>
-        <param name="trajectory_topic" value="$(arg trajectory_topic)"/>
-        <param name="vehicle_footprints_viz_topic" value="$(arg vehicle_footprints_viz_topic)"/>
-        <param name="collision_viz_topic" value="$(arg collision_viz_topic)"/>
+        <remap from="odom" to="$(arg odom_topic)"/>
+        <remap from="drive" to="$(arg drive_topic)"/>
     </node>
 </launch>

collision_warning_system/src/bicycle_model.cpp

@@ -8,10 +8,6 @@
 
 #include "collision_warning_system/bicycle_model.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 BicycleModel::BicycleModel(const double wheelbase) : wheelbase_(wheelbase)
 {
 }
@@ -84,5 +80,3 @@ geometry_msgs::PoseStamped BicycleModel::bicycleStateToPoseStamped(const Bicycle
 
   return pose_stamped;
 }
-}  // namespace safety
-}  // namespace f1tenth_racecar

collision_warning_system/src/bicycle_state.cpp

@@ -1,9 +1,5 @@
 #include "collision_warning_system/bicycle_state.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 BicycleState::BicycleState(double x, double y, double v, double yaw, double steering_angle)
 {
   x_ = x;
@@ -37,5 +33,3 @@ double BicycleState::steering_angle() const
 {
   return steering_angle_;
 }
-}  // namespace safety
-}  // namespace f1tenth_racecar

collision_warning_system/src/collision_warning_system.cpp

@@ -12,10 +12,6 @@
 #include "collision_warning_system/collision_warning_system.h"
 #include "f1tenth_utils/tf2_wrapper.h"
 
-namespace f1tenth_racecar
-{
-namespace safety
-{
 CollisionWarningSystem::CollisionWarningSystem()
 {
   ros::NodeHandle private_nh("~");
@@ -24,33 +20,23 @@ CollisionWarningSystem::CollisionWarningSystem()
   double circle_radius;
   double wheelbase;
 
-  std::string odom_topic;
-  std::string drive_topic;
-  std::string time_to_collision_topic;
-  std::string collision_viz_topic;
-
   private_nh.param("circle_offsets", circle_offsets, std::vector<double>{ 0.1, 0.3 });
   private_nh.param("circle_radius", circle_radius, 0.2);
   private_nh.getParam("t_max", t_max_);
   private_nh.getParam("delta_t", delta_t_);
 
-  private_nh.getParam("wheelbase", wheelbase);
-  private_nh.getParam("vehicle_width", vehicle_width_);
-  private_nh.getParam("vehicle_length", vehicle_length_);
-  private_nh.getParam("base_link_to_center_dist", base_link_to_center_dist_);
-
-  private_nh.getParam("odom_topic", odom_topic);
-  private_nh.getParam("drive_topic", drive_topic);
-  private_nh.getParam("time_to_collision_topic", time_to_collision_topic);
-  private_nh.getParam("collision_viz_topic", collision_viz_topic);
+  nh_.getParam("wheelbase", wheelbase);
+  nh_.getParam("vehicle_width", vehicle_width_);
+  nh_.getParam("vehicle_length", vehicle_length_);
+  nh_.getParam("base_link_to_center_dist", base_link_to_center_dist_);
 
   biycle_model_ = new BicycleModel(wheelbase);
   collision_checker_ = std::make_unique<CollisionChecker>(circle_offsets, circle_radius);
-  time_to_collision_pub_ = nh_.advertise<std_msgs::Float64>(time_to_collision_topic, 1);
-  viz_pub_ = nh_.advertise<visualization_msgs::Marker>(collision_viz_topic, 1);
+  time_to_collision_pub_ = nh_.advertise<std_msgs::Float64>("time_to_collision", 1);
+  viz_pub_ = nh_.advertise<visualization_msgs::Marker>("vizualization/collision_warning_system", 1);
 
-  odom_sub_ = nh_.subscribe(odom_topic, 1, &CollisionWarningSystem::odomCallback, this);
-  drive_sub_ = nh_.subscribe(drive_topic, 1, &CollisionWarningSystem::driveCallback, this);
+  odom_sub_ = nh_.subscribe("odom", 1, &CollisionWarningSystem::odomCallback, this);
+  drive_sub_ = nh_.subscribe("drive", 1, &CollisionWarningSystem::driveCallback, this);
   timer_ = nh_.createTimer(ros::Duration(0.1), &CollisionWarningSystem::timerCallback, this);
 }
 
@@ -82,13 +68,19 @@ void CollisionWarningSystem::timerCallback(const ros::TimerEvent& timer_event)
     }
 
     time_to_collision_pub_.publish(ttc);
-    ROS_WARN_COND(ttc.data < std::numeric_limits<double>::max(), "Time to collision: %.1f", ttc.data);
+    ROS_WARN_COND(ttc.data < std::numeric_limits<double>::max(), "[Collision Warning System] Time to collision: %.1f",
+                  ttc.data);
   }
   catch (const tf2::TransformException& ex)
   {
     ROS_WARN("%s", ex.what());
     return;
   }
+  catch (const std::runtime_error& ex)
+  {
+    ROS_WARN("%s", ex.what());
+    return;
+  }
 }
 
 void CollisionWarningSystem::odomCallback(const nav_msgs::Odometry& odom_msg)
@@ -171,13 +163,11 @@ void CollisionWarningSystem::visualizeProjectedTrajectory(const nav_msgs::Path&
 
   viz_pub_.publish(projected_marker);
 }
-}  // namespace safety
-}  // namespace f1tenth_racecar
 
 int main(int argc, char* argv[])
 {
   ros::init(argc, argv, "collision_warning_system");
-  f1tenth_racecar::safety::CollisionWarningSystem cws;
+  CollisionWarningSystem cws;
   ros::spin();
   return 0;
 }

costmap_generator/launch/costmap_generator.launch

@@ -1,16 +1,14 @@
 <launch>
-    <arg name="grid_size_x" value="10" />
-    <arg name="grid_size_y" value="10" />
-    <arg name="grid_resolution" value="0.05" />
-    <arg name="map_topic" value="map" />
-    <arg name="scan_topic" value="scan" />
+    <arg name="grid_size_x" default="10"/>
+    <arg name="grid_size_y" default="10"/>
+    <arg name="grid_resolution" default="0.05"/>
+    <arg name="scan_topic" default="scan"/>
 
     <node name="costmap_generator" pkg="costmap_generator" type="costmap_generator_node" output="screen">
-        <param name="grid_size_x" value="$(arg grid_size_x)" />
-        <param name="grid_size_y" value="$(arg grid_size_y)" />
-        <param name="grid_resolution" value="$(arg grid_resolution)" />
+        <param name="grid_size_x" value="$(arg grid_size_x)"/>
+        <param name="grid_size_y" value="$(arg grid_size_y)"/>
+        <param name="grid_resolution" value="$(arg grid_resolution)"/>
 
-        <param name="map_topic" value="$(arg map_topic)" />
-        <param name="scan_topic" value="$(arg scan_topic)" />
+        <remap from="scan" to="$(arg scan_topic)"/>
     </node>
 </launch>

costmap_generator/src/costmap_generator.cpp

@@ -24,18 +24,12 @@ CostmapGenerator::CostmapGenerator()
   double grid_size_y;
   double grid_resolution;
 
-  std::string map_topic;
-  std::string scan_topic;
-
   private_nh.param("grid_resolution", grid_resolution, 0.05);
   private_nh.param("grid_size_x", grid_size_x, 10.0);
   private_nh.param("grid_size_y", grid_size_y, 10.0);
 
-  private_nh.param("map_topic", map_topic, std::string("map"));
-  private_nh.param("scan_topic", scan_topic, std::string("scan"));
-
-  map_sub_ = nh_.subscribe(map_topic, 1, &CostmapGenerator::mapCallback, this);
-  scan_sub_ = nh_.subscribe(scan_topic, 1, &CostmapGenerator::scanCallback, this);
+  map_sub_ = nh_.subscribe("map", 1, &CostmapGenerator::mapCallback, this);
+  scan_sub_ = nh_.subscribe("scan", 1, &CostmapGenerator::scanCallback, this);
   costmap_pub_ = nh_.advertise<grid_map_msgs::GridMap>("costmap", 1, true);
   timer_ = nh_.createTimer(ros::Duration(0.1), &CostmapGenerator::timerCallback, this);
 

drive_controller/launch/drive_controller.launch

@@ -1,19 +1,15 @@
 <launch>
-    <arg name="look_ahead_dist" value="1" />
-    <arg name="gain" value="0.25" />
+    <arg name="look_ahead_dist" default="1"/>
+    <arg name="gain" default="0.40"/>
 
-    <arg name="trajectory_topic" value="path/local" />
-    <arg name="odom_topic" value="odom" />
-    <arg name="auto_drive_topic" value="drive" />
-    <arg name="viz_topic" value="drive_controller_viz" />
+    <arg name="trajectory_topic" default="trajectory"/>
+    <arg name="odom_topic" default="odom"/>
 
     <node name="drive_controller" pkg="drive_controller" type="drive_controller_node" output="screen">
-        <param name="look_ahead_dist" value="$(arg look_ahead_dist)" />
-        <param name="gain" value="$(arg gain)" />
+        <param name="look_ahead_dist" value="$(arg look_ahead_dist)"/>
+        <param name="gain" value="$(arg gain)"/>
 
-        <param name="trajectory_topic" value="$(arg trajectory_topic)" />
-        <param name="odom_topic" value="$(arg odom_topic)" />
-        <param name="auto_drive_topic" value="$(arg auto_drive_topic)" />
-        <param name="viz_topic" value="$(arg viz_topic)" />
+        <remap from="trajectory" to="$(arg trajectory_topic)"/>
+        <remap from="odom" to="$(arg odom_topic)"/>
     </node>
 </launch>

drive_controller/src/drive_controller.cpp

@@ -18,22 +18,13 @@ DriveController::DriveController()
   double look_ahead_dist;
   double gain;
 
-  std::string trajectory_topic;
-  std::string odom_topic;
-  std::string drive_topic;
-  std::string viz_topic;
-
   private_nh.getParam("look_ahead_dist", look_ahead_dist);
   private_nh.getParam("gain", gain);
-  private_nh.getParam("trajectory_topic", trajectory_topic);
-  private_nh.getParam("odom_topic", odom_topic);
-  private_nh.getParam("auto_drive_topic", drive_topic);
-  private_nh.getParam("viz_topic", viz_topic);
 
-  trajectory_sub_ = nh_.subscribe(trajectory_topic, 1, &DriveController::trajectoryCallback, this);
-  odom_sub_ = nh_.subscribe(odom_topic, 1, &DriveController::odomCallback, this);
-  drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>(drive_topic, 1);
-  viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(viz_topic, 1);
+  trajectory_sub_ = nh_.subscribe("trajectory", 1, &DriveController::trajectoryCallback, this);
+  odom_sub_ = nh_.subscribe("odom", 1, &DriveController::odomCallback, this);
+  drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>("drive_auto", 1);
+  viz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization/drive_controller", 1);
   timer_ = nh_.createTimer(ros::Duration(0.1), &DriveController::timerCallback, this);
 
   viz_ptr_ = std::make_shared<visualization_msgs::MarkerArray>();

drive_mode_selector/launch/drive_mode_selector.launch

@@ -1,13 +1,16 @@
 <launch>
-    <arg name="drive_mode_topic" value="drive_mode" />
-    <arg name="joy_drive_topic" value="drive/joy" />
-    <arg name="auto_drive_topic" value="drive/auto" />
-    <arg name="selected_drive_topic" value="drive/selected" />
+    <arg name="drive_mode_topic" default="drive_mode" />
+    <arg name="joy_drive_topic" default="drive_joy" />
+    <arg name="auto_drive_topic" default="drive_auto" />
 
     <node name="drive_mode_selector" pkg="drive_mode_selector" type="drive_mode_selector_node" output="screen">
         <param name="drive_mode_topic" value="$(arg drive_mode_topic)" />
         <param name="joy_drive_topic" value="$(arg joy_drive_topic)" />
         <param name="auto_drive_topic" value="$(arg auto_drive_topic)" />
         <param name="selected_drive_topic" value="$(arg selected_drive_topic)" />
+
+        <remap from="drive_mode_topic" to="$(arg drive_mode_topic)"/>
+        <remap from="joy_drive_topic" to="$(arg joy_drive_topic)"/>
+        <remap from="auto_drive_topic" to="$(arg auto_drive_topic)"/>
     </node>
 </launch>

drive_mode_selector/src/drive_mode_selector.cpp

@@ -12,21 +12,10 @@ DriveModeSelector::DriveModeSelector()
 {
   ros::NodeHandle private_nh("~");
 
-  std::string drive_mode_topic;
-  std::string joy_drive_topic;
-  std::string auto_drive_topic;
-  std::string selected_drive_topic;
-
-  getParam("drive_mode_topic", drive_mode_topic);
-  getParam("joy_drive_topic", joy_drive_topic);
-  getParam("auto_drive_topic", auto_drive_topic);
-  getParam("selected_drive_topic", selected_drive_topic);
-
-  drive_mode_sub_ = nh_.subscribe(drive_mode_topic, 1, &DriveModeSelector::driveModeCallback, this);
-  joy_drive_sub_ = nh_.subscribe(joy_drive_topic, 1, &DriveModeSelector::joyDriveCallback, this);
-  auto_drive_sub_ = nh_.subscribe(auto_drive_topic, 1, &DriveModeSelector::autoDriveCallback, this);
-
-  selected_drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>(selected_drive_topic, 1);
+  drive_mode_sub_ = nh_.subscribe("drive_mode", 1, &DriveModeSelector::driveModeCallback, this);
+  joy_drive_sub_ = nh_.subscribe("drive_joy", 1, &DriveModeSelector::joyDriveCallback, this);
+  auto_drive_sub_ = nh_.subscribe("drive_auto", 1, &DriveModeSelector::autoDriveCallback, this);
+  selected_drive_pub_ = nh_.advertise<ackermann_msgs::AckermannDriveStamped>("drive_selected", 1);
 
   drive_mode_ = Mode::PARK;
 }

ds4_controller/include/ds4_controller/ds4_controller.h

@@ -24,8 +24,6 @@ class DS4Controller
   ros::Publisher drive_pub_;
   ros::Publisher drive_mode_pub_;
 
-  std::string status_topic_;
-
   ros::Time last_message_time_;
   double timeout_;