initial slow code

urc_bringup/launch/bringup_simulation.launch.py

@@ -16,7 +16,7 @@
 
 def generate_launch_description():
     pkg_gazebo_ros = get_package_share_directory("gazebo_ros")
-    # pkg_urc_gazebo = get_package_share_directory("urc_gazebo")
+    pkg_urc_gazebo = get_package_share_directory("urc_gazebo")
     pkg_urc_bringup = get_package_share_directory("urc_bringup")
     pkg_path_planning = get_package_share_directory("path_planning")
     pkg_trajectory_following = get_package_share_directory("trajectory_following")
@@ -25,7 +25,7 @@ def generate_launch_description():
     controller_config_file_dir = os.path.join(
         pkg_urc_bringup, "config", "ros2_control_walli.yaml"
     )
-    # world_path = os.path.join(pkg_urc_gazebo, "urdf/worlds/urc_world.world")
+    world_path = os.path.join(pkg_urc_gazebo, "urdf/worlds/urc_world.world")
     use_sim_time = LaunchConfiguration("use_sim_time", default="true")
 
     xacro_file = os.path.join(
@@ -41,7 +41,7 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_gazebo_ros, "launch", "gazebo.launch.py"),
         ),
-        launch_arguments={"use_sim_time": "true"}.items(),  # "world": world_path
+        launch_arguments={"use_sim_time": "true",  "world": world_path}.items(),  
     )
 
     enable_color = SetEnvironmentVariable(name="RCUTILS_COLORIZED_OUTPUT", value="1")

urc_perception/src/elevation_mapping.cpp

@@ -126,7 +126,7 @@ namespace urc_perception
     map_.info.origin.position.z = 0.0;
     map_.info.origin.orientation.w = 1.0;
 
-    std::vector<int> relative_heights(map_.data.size());
+    std::vector<int> relative_heights(map_.data.size(), 0);
 
     for (unsigned int i = 0; i < cloud->size(); i++)
     {
@@ -140,8 +140,10 @@ namespace urc_perception
       relative_heights[j] = point.z - pos.z();
     }
 
-    int max_gradient = 0;                                // TODO: This is a hack to scale the gradients to a reasonable range.
-    std::vector<int> gradients(relative_heights.size()); // TODO:can collapse later into a single loop.
+
+
+    int max_gradient = 1;                                // TODO: This is a hack to scale the gradients to a reasonable range.
+    std::vector<int> gradients(relative_heights.size(), 0); // TODO:can collapse later into a single loop.
     int neighborhood_size = 3;
 
     for (unsigned int i = 0; i < relative_heights.size(); i++)
@@ -194,7 +196,8 @@ namespace urc_perception
 
       int costmap_index = map_coord.first + map_coord.second * map_.info.width;
 
-      double cost = gradients[i] / max_gradient * max_cost_;
+      double cost = gradients[costmap_index] / max_gradient * max_cost_;
+      // double cost = 0;
       if (cost > map_.data[costmap_index])
       {
         map_.data[costmap_index] = cost;