minor changes

Author: rlnav

launch/play_bag.launch

@@ -19,25 +19,27 @@
     </node>
 
     <!-- Debayering of images with image_proc -->
-    <group ns="camera_front">
-        <node name="debayer" pkg="nodelet" type="nodelet" args="standalone image_proc/debayer" respawn="false" output="screen">
-             <param name="image_transport" value="compressed"/>
-        </node>
-    </group>
-    <group ns="camera_left">
-        <node name="debayer" pkg="nodelet" type="nodelet" args="standalone image_proc/debayer" respawn="false" output="screen">
-             <param name="image_transport" value="compressed"/>
-        </node>
-    </group>
-    <group ns="camera_rear">
-        <node name="debayer" pkg="nodelet" type="nodelet" args="standalone image_proc/debayer" respawn="false" output="screen">
-             <param name="image_transport" value="compressed"/>
-        </node>
-    </group>
-    <group ns="camera_right">
-        <node name="debayer" pkg="nodelet" type="nodelet" args="standalone image_proc/debayer" respawn="false" output="screen">
-             <param name="image_transport" value="compressed"/>
-        </node>
+    <group>
+        <group ns="camera_front">
+            <node name="debayer" pkg="nodelet" type="nodelet" args="standalone image_proc/debayer" respawn="false" output="screen">
+                 <param name="image_transport" value="compressed"/>
+            </node>
+        </group>
+        <group ns="camera_left">
+            <node name="debayer" pkg="nodelet" type="nodelet" args="standalone image_proc/debayer" respawn="false" output="screen">
+                 <param name="image_transport" value="compressed"/>
+            </node>
+        </group>
+        <group ns="camera_rear">
+            <node name="debayer" pkg="nodelet" type="nodelet" args="standalone image_proc/debayer" respawn="false" output="screen">
+                 <param name="image_transport" value="compressed"/>
+            </node>
+        </group>
+        <group ns="camera_right">
+            <node name="debayer" pkg="nodelet" type="nodelet" args="standalone image_proc/debayer" respawn="false" output="screen">
+                 <param name="image_transport" value="compressed"/>
+            </node>
+        </group>
     </group>
 
     <!-- Visualization -->

nodes/hm_estimator

@@ -7,7 +7,6 @@ from cv_bridge import CvBridge
 from grid_map_msgs.msg import GridMap
 from monoforce.config import Config
 from monoforce.ros import height_map_to_point_cloud_msg, height_map_to_gridmap_msg
-from monoforce.utils import normalize, read_yaml
 from monoforce.cloudproc import estimate_heightmap, filter_grid, filter_range
 from monoforce.utils import position
 from ros_numpy import numpify, msgify
@@ -62,6 +61,10 @@ class HeightMapEstimator:
         t0 = time()
         # convert messages to points
         points = self.msgs_to_points(msgs)
+        if points is None:
+            rospy.logwarn('Could not convert messages to points')
+            return
+        rospy.logdebug('Points shape: %s' % str(points.shape))
 
         # estimate height map
         hm = estimate_heightmap(points, d_min=self.cfg.d_min, d_max=self.cfg.d_max,
@@ -74,13 +77,6 @@ class HeightMapEstimator:
         rospy.logdebug('Estimated height map shape: %s' % str(height.shape))
         rospy.logdebug('HM estimation time: %.3f' % (time() - t0))
 
-        # publish image
-        height_uint8 = np.asarray(255 * normalize(height), dtype='uint8')
-        img_msg = self.cv_bridge.cv2_to_imgmsg(height_uint8, encoding='mono8')
-        img_msg.header.stamp = msg.header.stamp
-        img_msg.header.frame_id = msg.header.frame_id
-        self.hm_img_pub.publish(img_msg)
-
         # publish point cloud
         t1 = time()
         cloud_msg = height_map_to_point_cloud_msg(height, self.cfg.grid_res,
@@ -118,11 +114,11 @@ class HeightMapEstimator:
                                                      time=msg.header.stamp,
                                                      timeout=rospy.Duration(0.5))
             except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
-                rospy.logwarn('Could not get transform from %s to %s' % (msg.header.frame_id, self.cfg.robot_frame))
-                return
+                rospy.logwarn('Could not get transform from %s to %s' % (msg.header.frame_id, self.robot_frame))
+                return None
+
             Tr = numpify(tf.transform).reshape((4, 4))
             points = np.matmul(Tr[:3, :3], points.T).T + Tr[:3, 3]
-
             all_points.append(points)
 
         all_points = np.concatenate(all_points, axis=0)

scripts/create_lss_depth_data

@@ -5,6 +5,10 @@
 # It also adds calibrations, colors and velocities to the point clouds
 
 DATA_PATH=/home/$USER/data/robingas/data
+if [ ! -d "$DATA_PATH" ]; then
+    echo "Data path does not exist: $DATA_PATH"
+    exit 1
+fi
 INPUT_DATA_STEP=20
 IMU_TOPIC='/imu/data'
 CONTROL_MODEL='diffdrive'
@@ -22,11 +26,19 @@ SEQUENCES=(
 )
 
 # source ROS workspace
+if [ ! -d "/home/$USER/workspaces/traversability_ws" ]; then
+    echo "ROS workspace does not exist: /home/$USER/workspaces/traversability_ws"
+    exit 1
+fi
 source /home/$USER/workspaces/traversability_ws/devel/setup.bash
 
 # loop through bag files
 for SEQ in "${SEQUENCES[@]}"
 do
+    if [ ! -f "${SEQ}.bag" ]; then
+        echo "Bag file does not exist: ${SEQ}.bag"
+        continue
+    fi
     echo "Processing bag file: ${SEQ}.bag"
     # if husky in bag file name, use cloud topic /points
     if [[ $SEQ == *"husky"* ]]; then