diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py
index 6a3d2862..cbe67fb8 100644
--- a/hello_helpers/src/hello_helpers/hello_misc.py
+++ b/hello_helpers/src/hello_helpers/hello_misc.py
@@ -140,32 +140,38 @@ def get_robot_floor_pose_xya(self, floor_frame='odom'):
def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
rospy.init_node(node_name)
self.node_name = rospy.get_name()
- rospy.loginfo("{0} started".format(self.node_name))
+ rospy.loginfo(f"{self.node_name} started")
- self.trajectory_client = actionlib.SimpleActionClient('/stretch_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
+ fjt_actionserver_name = '/stretch_controller/follow_joint_trajectory'
+ rospy.logdebug(f'{self.node_name} waiting to connect to action server {fjt_actionserver_name}')
+ self.trajectory_client = actionlib.SimpleActionClient(fjt_actionserver_name, FollowJointTrajectoryAction)
server_reached = self.trajectory_client.wait_for_server(timeout=rospy.Duration(60.0))
if not server_reached:
- rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.')
+ rospy.logerr(f'{self.node_name} unable to connect to robot action server. Timeout exceeded.')
+ rospy.signal_shutdown('Unable to connect to robot action server. Timeout exceeded.')
sys.exit()
-
+ rospy.logdebug(f'{self.node_name} connected to {fjt_actionserver_name}')
+
self.tf2_buffer = tf2_ros.Buffer()
self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
-
+
self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback)
self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1)
- rospy.wait_for_service('/stop_the_robot')
- rospy.loginfo('Node ' + self.node_name + ' connected to /stop_the_robot service.')
- self.stop_the_robot_service = rospy.ServiceProxy('/stop_the_robot', Trigger)
-
+ stop_robot_service_name = '/stop_the_robot'
+ rospy.logdebug(f'{self.node_name} waiting to connect to service {stop_robot_service_name}')
+ rospy.wait_for_service(stop_robot_service_name)
+ self.stop_the_robot_service = rospy.ServiceProxy(stop_robot_service_name, Trigger)
+ rospy.logdebug(f'{self.node_name} connected to {stop_robot_service_name}')
+
if wait_for_first_pointcloud:
# Do not start until a point cloud has been received
point_cloud_msg = self.point_cloud
- print('Node ' + node_name + ' waiting to receive first point cloud.')
+ rospy.logdebug(f'{self.node_name} waiting to receive first point cloud')
while point_cloud_msg is None:
rospy.sleep(0.1)
point_cloud_msg = self.point_cloud
- print('Node ' + node_name + ' received first point cloud, so continuing.')
+ rospy.logdebug(f'{self.node_name} received first point cloud, so continuing')
def create_time_string():
diff --git a/stretch_core/config/stretch_marker_dict.yaml b/stretch_core/config/stretch_marker_dict.yaml
index 981014f7..f5b6506b 100644
--- a/stretch_core/config/stretch_marker_dict.yaml
+++ b/stretch_core/config/stretch_marker_dict.yaml
@@ -37,6 +37,11 @@
'use_rgb_only': False
'name': 'shoulder_top'
'link': 'link_aruco_shoulder'
+ '245':
+ 'length_mm': 88.0
+ 'use_rgb_only': False
+ 'name': 'link_docking_station'
+ 'link': None
'246':
'length_mm': 179.0
'use_rgb_only': False
diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop
index cf1b96a8..0bd032ec 100755
--- a/stretch_core/nodes/keyboard_teleop
+++ b/stretch_core/nodes/keyboard_teleop
@@ -15,13 +15,14 @@ import hello_helpers.hello_misc as hm
class GetKeyboardCommands:
- def __init__(self, mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on):
+ def __init__(self, mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on, docking_on):
self.mapping_on = mapping_on
self.hello_world_on = hello_world_on
self.open_drawer_on = open_drawer_on
self.clean_surface_on = clean_surface_on
self.grasp_object_on = grasp_object_on
self.deliver_object_on = deliver_object_on
+ self.docking_on = docking_on
self.step_size = 'medium'
self.rad_per_deg = math.pi/180.0
@@ -210,6 +211,11 @@ class GetKeyboardCommands:
trigger_result = node.trigger_deliver_object_service(trigger_request)
rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+ if ((c == 'n') or (c == 'N')) and self.docking_on:
+ trigger_request = TriggerRequest()
+ trigger_result = node.trigger_dock_robot_service(trigger_request)
+ rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+
####################################################
## BASIC KEYBOARD TELEOPERATION COMMANDS
@@ -294,9 +300,9 @@ class GetKeyboardCommands:
class KeyboardTeleopNode(hm.HelloNode):
- def __init__(self, mapping_on=False, hello_world_on=False, open_drawer_on=False, clean_surface_on=False, grasp_object_on=False, deliver_object_on=False):
+ def __init__(self, mapping_on=False, hello_world_on=False, open_drawer_on=False, clean_surface_on=False, grasp_object_on=False, deliver_object_on=False, docking_on=False):
hm.HelloNode.__init__(self)
- self.keys = GetKeyboardCommands(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on)
+ self.keys = GetKeyboardCommands(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on, docking_on)
self.rate = 10.0
self.joint_state = None
self.mapping_on = mapping_on
@@ -305,6 +311,7 @@ class KeyboardTeleopNode(hm.HelloNode):
self.clean_surface_on = clean_surface_on
self.grasp_object_on = grasp_object_on
self.deliver_object_on = deliver_object_on
+ self.docking_on = docking_on
def joint_states_callback(self, joint_state):
self.joint_state = joint_state
@@ -396,6 +403,11 @@ class KeyboardTeleopNode(hm.HelloNode):
rospy.loginfo('Node ' + self.node_name + ' connected to /deliver_object/trigger_deliver_object.')
self.trigger_deliver_object_service = rospy.ServiceProxy('/deliver_object/trigger_deliver_object', Trigger)
+ if self.docking_on:
+ rospy.wait_for_service('/dock_robot/trigger_dock_robot')
+ rospy.loginfo('Node ' + self.node_name + ' connected to /dock_robot/trigger_dock_robot.')
+ self.trigger_dock_robot_service = rospy.ServiceProxy('/dock_robot/trigger_dock_robot', Trigger)
+
rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
rate = rospy.Rate(self.rate)
@@ -418,6 +430,7 @@ if __name__ == '__main__':
parser.add_argument('--clean_surface_on', action='store_true', help='Enable Clean Surface trigger, which requires connection to the appropriate clean_surface service.')
parser.add_argument('--grasp_object_on', action='store_true', help='Enable Grasp Object trigger, which requires connection to the appropriate grasp_object service.')
parser.add_argument('--deliver_object_on', action='store_true', help='Enable Deliver Object trigger, which requires connection to the appropriate deliver_object service.')
+ parser.add_argument('--docking_on', action='store_true', help='Enable Dock Robot trigger, which requires connection to the appropriate dock_robot service.')
args, unknown = parser.parse_known_args()
mapping_on = args.mapping_on
@@ -426,8 +439,9 @@ if __name__ == '__main__':
clean_surface_on = args.clean_surface_on
grasp_object_on = args.grasp_object_on
deliver_object_on = args.deliver_object_on
+ docking_on = args.docking_on
- node = KeyboardTeleopNode(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on)
+ node = KeyboardTeleopNode(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on, docking_on)
node.main()
except KeyboardInterrupt:
rospy.loginfo('interrupt received, so shutting down')
diff --git a/stretch_demos/launch/dock_robot.launch b/stretch_demos/launch/dock_robot.launch
new file mode 100644
index 00000000..e5b0efd8
--- /dev/null
+++ b/stretch_demos/launch/dock_robot.launch
@@ -0,0 +1,45 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/stretch_demos/nodes/dock_robot b/stretch_demos/nodes/dock_robot
new file mode 100755
index 00000000..3dbec2fb
--- /dev/null
+++ b/stretch_demos/nodes/dock_robot
@@ -0,0 +1,117 @@
+#!/usr/bin/env python3
+
+import rospy
+from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
+from sensor_msgs.msg import BatteryState
+
+import copy
+import threading
+import argparse as ap
+import hello_helpers.hello_misc as hm
+import stretch_body.hello_utils as hu
+
+
+class DockRobotNode(hm.HelloNode):
+
+ def __init__(self):
+ hm.HelloNode.__init__(self)
+ self.rate = 10.0
+ self.debug_directory = None
+ self.battery_state = None
+ self.battery_state_lock = threading.Lock()
+
+ def battery_state_callback(self, battery_state):
+ with self.battery_state_lock:
+ self.battery_state = battery_state
+
+ def trigger_dock_robot_callback(self, request):
+ # 1. Check predocking conditions (e.g. not already docked, arm stowed, etc.)
+ with self.battery_state_lock:
+ battery_state = copy.copy(self.battery_state)
+ rospy.logdebug(f'{self.node_name} got battery_state={battery_state}')
+ if battery_state.present:
+ rospy.logerr('Already plugged into charger')
+ return TriggerResponse(
+ success=False,
+ message='Already plugged into charger'
+ )
+ if battery_state.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_CHARGING:
+ rospy.logerr('Already charging')
+ return TriggerResponse(
+ success=False,
+ message='Already charging'
+ )
+ stow_trigger_request = TriggerRequest()
+ stow_trigger_result = self.trigger_stow_the_robot_service(stow_trigger_request)
+ if not stow_trigger_result.success:
+ rospy.logerr(f'Robot driver failed to stow arm with msg={stow_trigger_result.message}')
+ return TriggerResponse(
+ success=False,
+ message=stow_trigger_result.message
+ )
+ position_mode_trigger_request = TriggerRequest()
+ position_mode_trigger_result = self.trigger_position_mode_service(position_mode_trigger_request)
+ if not position_mode_trigger_result.success:
+ rospy.logerr(f'Robot driver failed to switch into position mode with msg={position_mode_trigger_result.message}')
+ return TriggerResponse(
+ success=False,
+ message=position_mode_trigger_result.message
+ )
+
+ # 2. Perform a head scan, looking for the docking station's ArUco marker
+ # 3. Ask FUNMAP to plan and execute the mobile base to a pose in front of the docking station
+ # 4. Back up until charging detected
+
+ return TriggerResponse(
+ success=False,
+ message='Not Implemented'
+ )
+
+ def main(self):
+ hm.HelloNode.main(self, 'dock_robot', 'dock_robot', wait_for_first_pointcloud=False)
+
+ self.debug_directory = rospy.get_param('~debug_directory', hu.get_stretch_directory('debug'))
+ rospy.logdebug(f'Using the following directory for debugging files: {self.debug_directory}')
+
+ self.battery_state_subscriber = rospy.Subscriber('/battery', BatteryState, self.battery_state_callback)
+
+ position_mode_service_name = '/switch_to_position_mode'
+ rospy.logdebug(f'{self.node_name} waiting to connect to service {position_mode_service_name}')
+ rospy.wait_for_service(position_mode_service_name)
+ self.trigger_position_mode_service = rospy.ServiceProxy(position_mode_service_name, Trigger)
+ rospy.logdebug(f'{self.node_name} connected to {position_mode_service_name}')
+
+ stow_service_name = '/stow_the_robot'
+ rospy.logdebug(f'{self.node_name} waiting to connect to service {stow_service_name}')
+ rospy.wait_for_service(stow_service_name)
+ self.trigger_stow_the_robot_service = rospy.ServiceProxy(stow_service_name, Trigger)
+ rospy.logdebug(f'{self.node_name} connected to {stow_service_name}')
+
+ default_camera_preset_service_name = '/camera/switch_to_default_mode'
+ high_accuracy_camera_preset_service_name = '/camera/switch_to_high_accuracy_mode'
+ rospy.logdebug(f'{self.node_name} waiting to connect to services {default_camera_preset_service_name} and {high_accuracy_camera_preset_service_name}')
+ rospy.wait_for_service(default_camera_preset_service_name)
+ self.trigger_d435i_default_preset_service = rospy.ServiceProxy(default_camera_preset_service_name, Trigger)
+ rospy.logdebug(f'{self.node_name} connected to {default_camera_preset_service_name}')
+ rospy.wait_for_service(high_accuracy_camera_preset_service_name)
+ self.trigger_d435i_high_accuracy_preset_service = rospy.ServiceProxy(high_accuracy_camera_preset_service_name, Trigger)
+ rospy.logdebug(f'{self.node_name} connected to {high_accuracy_camera_preset_service_name}')
+
+ self.trigger_grasp_object_service = rospy.Service('/dock_robot/trigger_dock_robot',
+ Trigger,
+ self.trigger_dock_robot_callback)
+
+ rospy.loginfo(f'{self.node_name} ready!')
+ rate = rospy.Rate(self.rate)
+ while not rospy.is_shutdown():
+ rate.sleep()
+
+
+if __name__ == '__main__':
+ try:
+ parser = ap.ArgumentParser(description='Dock Robot behavior for stretch.')
+ args, unknown = parser.parse_known_args()
+ node = DockRobotNode()
+ node.main()
+ except KeyboardInterrupt:
+ rospy.loginfo('interrupt received, so shutting down')
diff --git a/stretch_demos/rviz/dock_robot.rviz b/stretch_demos/rviz/dock_robot.rviz
new file mode 100644
index 00000000..ced2fab8
--- /dev/null
+++ b/stretch_demos/rviz/dock_robot.rviz
@@ -0,0 +1,492 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded: ~
+ Splitter Ratio: 0.5
+ Tree Height: 1079
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: PointCloud2
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera_accel_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_accel_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_gyro_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_gyro_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_infra1_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_infra1_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_infra2_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_infra2_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ laser:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_inner_wrist:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_left_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_right_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_shoulder:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_top_wrist:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_gripper_finger_left:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_gripper_finger_right:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_gripper_fingertip_left:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_gripper_fingertip_right:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_head:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_head_pan:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_head_tilt:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_left_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_lift:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_mast:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_right_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_straight_gripper:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_wrist_pitch:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_wrist_roll:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_wrist_yaw:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_wrist_yaw_bottom:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ respeaker_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 1.1299999952316284
+ Min Value: -0.05000000074505806
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: AxisColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic: /funmap/point_cloud2
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/LaserScan
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: LaserScan
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic: /scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /funmap/navigation_plan_markers
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: false
+ Marker Topic: /funmap/voi_marker
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/Marker
+ Enabled: false
+ Marker Topic: /frustum_marker/depth_camera
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/Marker
+ Enabled: false
+ Marker Topic: /frustum_marker/color_camera
+ Name: Marker
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ - Alpha: 0.20000000298023224
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.004000000189989805
+ Style: Flat Squares
+ Topic: /camera/depth/color/points
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Angle Tolerance: 0.009999999776482582
+ Class: rviz/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: false
+ Enabled: false
+ Keep: 1
+ Name: Odometry
+ Position Tolerance: 0.009999999776482582
+ Queue Size: 10
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic: /odometry/filtered
+ Unreliable: false
+ Value: false
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 10
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.7853981852531433
+ Target Frame:
+ Yaw: 0.7853981852531433
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1376
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001c2000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000838000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 2560
+ X: 0
+ Y: 27