Formatting

src/agent/localization/stretch_ros_move_api.py

@@ -8,7 +8,13 @@
 from sensor_msgs.msg import JointState
 from control_msgs.msg import FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
-from geometry_msgs.msg import Pose, PoseStamped, Pose2D, PoseWithCovarianceStamped, Twist
+from geometry_msgs.msg import (
+    Pose,
+    PoseStamped,
+    Pose2D,
+    PoseWithCovarianceStamped,
+    Twist,
+)
 from nav_msgs.msg import Odometry
 import hello_helpers.hello_misc as hm
 from tf.transformations import euler_from_quaternion
@@ -114,7 +120,9 @@ def _odom_callback(self, pose):
         w = cutoff_angle(t_curr - self._t_odom_prev, SLAM_CUTOFF_HZ)
         coeff = 1 / (w + 1)
 
-        pose_diff_log = coeff * pose_diff_odom.log() + (1 - coeff) * pose_diff_slam.log()
+        pose_diff_log = (
+            coeff * pose_diff_odom.log() + (1 - coeff) * pose_diff_slam.log()
+        )
         self._filtered_pose = self._filtered_pose * sp.SE3.exp(pose_diff_log)
         self._publish_filtered_state(ros_time)
 
@@ -216,7 +224,9 @@ def _send_command(self, joint_name, increment):
         self.trajectory_client.send_goal(trajectory_goal)
 
         rospy.loginfo(
-            "joint_name = {0}, trajectory_goal = {1}".format(joint_name, trajectory_goal)
+            "joint_name = {0}, trajectory_goal = {1}".format(
+                joint_name, trajectory_goal
+            )
         )
         rospy.loginfo("Done sending pose.")
 
@@ -230,14 +240,22 @@ def stop(self):
     def background_loop(self):
 
         rospy.Subscriber(
-            "/stretch/joint_states", JointState, self._joint_states_callback, queue_size=1
+            "/stretch/joint_states",
+            JointState,
+            self._joint_states_callback,
+            queue_size=1,
         )
         # This comes from hector_slam. It's a transform from src_frame = 'base_link', target_frame = 'map'
         rospy.Subscriber(
-            "/poseupdate", PoseWithCovarianceStamped, self._slam_pose_callback, queue_size=1
+            "/poseupdate",
+            PoseWithCovarianceStamped,
+            self._slam_pose_callback,
+            queue_size=1,
         )
         # this comes from lidar matching, i.e. no slam/global-optimization
-        rospy.Subscriber("/pose2D", Pose2D, self._scan_matched_pose_callback, queue_size=1)
+        rospy.Subscriber(
+            "/pose2D", Pose2D, self._scan_matched_pose_callback, queue_size=1
+        )
         # This comes from wheel odometry.
         rospy.Subscriber("/odom", Odometry, self._odom_callback, queue_size=1)
 
@@ -254,7 +272,10 @@ def background_loop(self):
 
     def start(self):
         hm.HelloNode.main(
-            self, "fairo_hello_proxy", "fairo_hello_proxy", wait_for_first_pointcloud=False
+            self,
+            "fairo_hello_proxy",
+            "fairo_hello_proxy",
+            wait_for_first_pointcloud=False,
         )
         self._thread = threading.Thread(target=self.background_loop, daemon=True)
         self._thread.start()

src/agent/perception/remote_hello_realsense.py

@@ -92,20 +92,27 @@ def is_lidar_obstacle(self):
     def _connect_to_realsense(self):
         if self.use_ros_realsense:
             print("Creating cameras...")
-            self.rgb_cam = RosCamera('/camera/color')
-            self.dpt_cam = RosCamera('/camera/aligned_depth_to_color', buffer_size=5)
+            self.rgb_cam = RosCamera("/camera/color")
+            self.dpt_cam = RosCamera("/camera/aligned_depth_to_color", buffer_size=5)
 
             print("Waiting for camera images...")
             self.rgb_cam.wait_for_image()
             self.dpt_cam.wait_for_image()
 
-            self.intrinsic_mat = np.array([
-                [self.rgb_cam.fx, 0, self.rgb_cam.px],
-                [0, self.rgb_cam.fy, self.rgb_cam.py],
-                [0, 0, 1]]
+            self.intrinsic_mat = np.array(
+                [
+                    [self.rgb_cam.fx, 0, self.rgb_cam.px],
+                    [0, self.rgb_cam.fy, self.rgb_cam.py],
+                    [0, 0, 1],
+                ]
             )
             self.intrinsic_o3d = o3d.camera.PinholeCameraIntrinsic(
-                CW, CH, self.rgb_cam.fx, self.rgb_cam.fy, self.rgb_cam.px, self.rgb_cam.py
+                CW,
+                CH,
+                self.rgb_cam.fx,
+                self.rgb_cam.fy,
+                self.rgb_cam.px,
+                self.rgb_cam.py,
             )
 
         else:
@@ -127,8 +134,12 @@ def _connect_to_realsense(self):
             # we need to use the intrinsics of the color frame
             color_profile = rs.video_stream_profile(profile.get_stream(rs.stream.color))
             i = color_profile.get_intrinsics()
-            self.intrinsic_mat = np.array([[i.fx, 0, i.ppx], [0, i.fy, i.ppy], [0, 0, 1]])
-            self.intrinsic_o3d = o3d.camera.PinholeCameraIntrinsic(CW, CH, i.fx, i.fy, i.ppx, i.ppy)
+            self.intrinsic_mat = np.array(
+                [[i.fx, 0, i.ppx], [0, i.fy, i.ppy], [0, 0, 1]]
+            )
+            self.intrinsic_o3d = o3d.camera.PinholeCameraIntrinsic(
+                CW, CH, i.fx, i.fy, i.ppx, i.ppy
+            )
 
             align_to = rs.stream.color
             self.align = rs.align(align_to)
@@ -161,8 +172,8 @@ def test_connection(self):
     def get_rgb_depth(self, rotate=True, compressed=False):
         if self.use_ros_realsense:
             depth_image = self.dpt_cam.get_filtered()
-            depth_image[depth_image < 0.1] = 0.
-            depth_image[depth_image > 4.0] = 0.
+            depth_image[depth_image < 0.1] = 0.0
+            depth_image[depth_image > 4.0] = 0.0
             color_image = self.rgb_cam.get()
 
         else:
@@ -224,7 +235,9 @@ def depth_to_xyz(depth):
         xyz = xyz.reshape(-1, 3)
         return xyz
 
-    def get_rgb_depth_optimized_for_habitat_transfer(self, rotate=True, compressed=False):
+    def get_rgb_depth_optimized_for_habitat_transfer(
+        self, rotate=True, compressed=False
+    ):
         tm = time.time()
         frames = None
         while not frames:
@@ -335,7 +348,9 @@ def get_open3d_pcd(self, rgb_depth=None, cam_transform=None, base_state=None):
         extrinsic = np.linalg.inv(final_transform)
         # create point cloud
 
-        opcd = o3d.geometry.PointCloud.create_from_rgbd_image(orgbd, intrinsic, extrinsic)
+        opcd = o3d.geometry.PointCloud.create_from_rgbd_image(
+            orgbd, intrinsic, extrinsic
+        )
         return opcd
 
     def get_current_pcd(self):

src/hw/remote_hello_robot_ros.py

@@ -68,7 +68,8 @@ def set_tilt_correction(self, angle):
         angle in radians
         """
         print(
-            "[hello-robot] Setting tilt correction " "to angle: {} degrees".format(degrees(angle))
+            "[hello-robot] Setting tilt correction "
+            "to angle: {} degrees".format(degrees(angle))
         )
 
         self.tilt_correction = angle
@@ -251,7 +252,9 @@ def obstacle_fn():
                 return self.cam.is_obstacle_in_front()
 
             try:
-                status = goto(self, list(xyt_position), dryrun=False, obstacle_fn=obstacle_fn)
+                status = goto(
+                    self, list(xyt_position), dryrun=False, obstacle_fn=obstacle_fn
+                )
                 self._done = True
             except Exception as e:
                 print(e)

src/hw/stretch_ros_move_api.py

@@ -121,7 +121,9 @@ def _send_command(self, joint_name, increment):
         self.trajectory_client.send_goal(trajectory_goal)
 
         rospy.loginfo(
-            "joint_name = {0}, trajectory_goal = {1}".format(joint_name, trajectory_goal)
+            "joint_name = {0}, trajectory_goal = {1}".format(
+                joint_name, trajectory_goal
+            )
         )
         rospy.loginfo("Done sending pose.")
 
@@ -135,14 +137,22 @@ def stop(self):
     def background_loop(self):
 
         rospy.Subscriber(
-            "/stretch/joint_states", JointState, self._joint_states_callback, queue_size=1
+            "/stretch/joint_states",
+            JointState,
+            self._joint_states_callback,
+            queue_size=1,
         )
         # This comes from hector_slam. It's a transform from src_frame = 'base_link', target_frame = 'map'
         rospy.Subscriber(
-            "/poseupdate", PoseWithCovarianceStamped, self._slam_pose_callback, queue_size=1
+            "/poseupdate",
+            PoseWithCovarianceStamped,
+            self._slam_pose_callback,
+            queue_size=1,
         )
         # this comes from lidar matching, i.e. no slam/global-optimization
-        rospy.Subscriber("/pose2D", Pose2D, self._scan_matched_pose_callback, queue_size=1)
+        rospy.Subscriber(
+            "/pose2D", Pose2D, self._scan_matched_pose_callback, queue_size=1
+        )
         # This comes from wheel odometry.
         rospy.Subscriber("/odom", Odometry, self._odom_callback, queue_size=1)
 
@@ -159,7 +169,10 @@ def background_loop(self):
 
     def start(self):
         hm.HelloNode.main(
-            self, "fairo_hello_proxy", "fairo_hello_proxy", wait_for_first_pointcloud=False
+            self,
+            "fairo_hello_proxy",
+            "fairo_hello_proxy",
+            wait_for_first_pointcloud=False,
         )
         self._thread = threading.Thread(target=self.background_loop, daemon=True)
         self._thread.start()