stretch_ros/stretch_funmap/nodes/funmap

#!/usr/bin/env python

from __future__ import print_function

import rospy
import actionlib

from sensor_msgs.msg import JointState
from geometry_msgs.msg import Transform, TransformStamped, PoseWithCovarianceStamped, PoseStamped, Pose, PointStamped
from nav_msgs.msg import Odometry
from move_base_msgs.msg import MoveBaseAction, MoveBaseResult, MoveBaseFeedback
from nav_msgs.srv import GetPlan
from nav_msgs.msg import Path
from sensor_msgs.msg import PointCloud2
from visualization_msgs.msg import Marker, MarkerArray
from std_srvs.srv import Trigger, TriggerResponse, TriggerRequest
from tf.transformations import euler_from_quaternion
from tf2_geometry_msgs import do_transform_pose

import numpy as np
import scipy.ndimage as nd
import cv2
import math
import time
import threading
import sys
import os
import copy

import tf_conversions
import ros_numpy
import tf2_ros

import argparse as ap

import hello_helpers.hello_misc as hm
import hello_helpers.hello_ros_viz as hr

import stretch_funmap.merge_maps as mm
import stretch_funmap.navigate as nv
import stretch_funmap.mapping as ma
import stretch_funmap.segment_max_height_image as sm
import stretch_funmap.navigation_planning as na
import stretch_funmap.manipulation_planning as mp

def create_map_to_odom_transform(t_mat):
    t = TransformStamped()
    t.header.stamp = rospy.Time.now()
    t.header.frame_id = 'map'
    t.child_frame_id = 'odom'
    t.transform = ros_numpy.msgify(Transform, t_mat)
    return t


class ContactDetector():
    def __init__(self, get_joint_state, in_contact_func, move_increment=0.008):
        self.in_contact_func = in_contact_func

        # reach until contact related
        self.in_contact = False
        self.in_contact_position = None
        self.contact_state_lock = threading.Lock()
        self.contact_mode = None
        self.contact_mode_lock = threading.Lock()
        
        self.position = None
        self.av_effort = None
        self.av_effort_window_size = 3

        self.get_joint_state = get_joint_state        
        self.direction_sign = None
        self.stopping_position = None

        self.min_av_effort_threshold = 10.0
        self.move_increment = move_increment

    def set_regulate_contact(self):
        with self.contact_mode_lock:
            return self.contact_mode == 'regulate_contact'
        
    def set_stopping_position(self, stopping_position, direction_sign):
        assert((direction_sign == -1) or (direction_sign == 1))
        self.stopping_position = stopping_position
        self.direction_sign = direction_sign

    def is_in_contact(self):
        with self.contact_state_lock:
            return self.in_contact

    def contact_position(self):
        with self.contact_state_lock:
            return self.in_contact_position
         
    def get_position(self):
        return self.position
    
    def passed_stopping_position(self):
        if (self.position is None) or (self.stopping_position is None):
            return False
        difference = self.stopping_position - self.position
        if int(np.sign(difference)) == self.direction_sign:
            return False
        return True

    def not_stopped(self):
        with self.contact_mode_lock:
            return self.contact_mode == 'stop_on_contact'
    
    def reset(self): 
        with self.contact_state_lock:
            self.in_contact = False
            self.in_contact_position = None
        self.turn_off()
        self.stopping_position = None
        self.direction_sign = None
        
    def turn_off(self):
        with self.contact_mode_lock:
            self.contact_mode = None

    def turn_on(self):
        with self.contact_mode_lock:
            self.contact_mode = 'stop_on_contact'

    def update(self, joint_states, stop_the_robot_service):
        with self.contact_state_lock:
            self.in_contact = False
            self.in_contact_wrist_position = None

        position, velocity, effort = self.get_joint_state(joint_states)
        self.position = position

        # First, check that the stopping position, if defined, has not been passed
        if self.passed_stopping_position(): 
	    trigger_request = TriggerRequest()
	    trigger_result = stop_the_robot_service(trigger_request)
            with self.contact_mode_lock:
                self.contact_mode = 'passed_stopping_point'
            rospy.loginfo('stop_on_contact: stopping the robot due to passing the stopping position, position = {0}, stopping_position = {1}, direction_sign = {2}'.format(self.position, self.stopping_position, self.direction_sign))

        # Second, check that the effort thresholds have not been exceeded
        if self.av_effort is None:
            self.av_effort = effort
        else:
            self.av_effort = (((self.av_effort_window_size - 1.0) * self.av_effort) + effort) / self.av_effort_window_size

        if self.in_contact_func(effort, self.av_effort):
            # Contact detected!
            with self.contact_state_lock:
                self.in_contact = True
                self.in_contact_position = self.position
            with self.contact_mode_lock:
                if self.contact_mode == 'stop_on_contact':
	            trigger_request = TriggerRequest()
	            trigger_result = stop_the_robot_service(trigger_request)
                    rospy.loginfo('stop_on_contact: stopping the robot due to detected contact, effort = {0}, av_effort = {1}'.format(effort, self.av_effort))
                    self.contact_mode = 'regulate_contact'
                elif self.contact_mode == 'regulate_contact':
                    pass
        elif self.av_effort < self.min_av_effort_threshold:
            with self.contact_mode_lock:
                if self.contact_mode == 'regulate_contact':
                    pass
        else:
            pass

            
    def move_until_contact(self, joint_name, stopping_position, direction_sign, move_to_pose):
        self.reset()
        self.set_stopping_position(stopping_position, direction_sign)
        
        success = False
        message = 'Unknown result.'
        
        if not self.passed_stopping_position():
            # The target has not been passed
            self.turn_on()

            move_rate = rospy.Rate(5.0)
            move_increment = direction_sign * self.move_increment
            finished = False
            while self.not_stopped():
                position = self.get_position()
                if position is not None: 
                    new_target = self.get_position() + move_increment
                    pose = {joint_name : new_target}
                    move_to_pose(pose, async=True)
                move_rate.sleep()

            if self.is_in_contact():
                # back off from the detected contact location
                
                contact_position = self.contact_position()
                if contact_position is not None: 
                    new_target = contact_position - 0.001 #- 0.002
                else:
                    new_target = self.position() - 0.001 #- 0.002
                pose = {joint_name : new_target}
                move_to_pose(pose, async=False)
                rospy.loginfo('backing off after contact: moving away from surface to decrease force')
                success = True
                message = 'Successfully reached until contact.'
            else: 
                success = False
                message = 'Terminated without detecting contact.'

        self.reset()

        return success, message

    
    
class FunmapNode(hm.HelloNode):

    def __init__(self, map_filename):
        hm.HelloNode.__init__(self)

        self.map_filename = map_filename
        self.debug_directory = None
        
        # This holds all the poses the robot's mobile base was in
        # while making scans merged into the map. They are defined
        # with respect to the map's image. One use of this list is to
        # fill in the robot's footprints as floor when producing a
        # floor mask for the purposes of navigations with the
        # assumption that the robot's base will only be on traversable
        # floor.
        self.robot_poses = []
        self.prev_nav_markers = None
    
        self.wrist_position = None
        
        self.use_hook = False #True #False

        if self.use_hook: 
            def extension_contact_func(effort, av_effort):
                single_effort_threshold = 38.0
                av_effort_threshold = 34.0

                if (effort >= single_effort_threshold):
                    rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format(effort, single_effort_threshold))
                if (av_effort >= av_effort_threshold):
                    rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format(av_effort, av_effort_threshold))

                return ((effort >= single_effort_threshold) or
                        (av_effort > av_effort_threshold))

            self.extension_contact_detector = ContactDetector(hm.get_wrist_state, extension_contact_func, move_increment=0.008)

        else:
            def extension_contact_func(effort, av_effort):
                single_effort_threshold = 40.0
                av_effort_threshold = 40.0

                if (effort >= single_effort_threshold):
                    rospy.loginfo('Extension single effort exceeded single_effort_threshold: {0} >= {1}'.format(effort, single_effort_threshold))
                if (av_effort >= av_effort_threshold):
                    rospy.loginfo('Extension average effort exceeded av_effort_threshold: {0} >= {1}'.format(av_effort, av_effort_threshold))
                
                return ((effort >= single_effort_threshold) or
                        (av_effort > av_effort_threshold))
            
            self.extension_contact_detector = ContactDetector(hm.get_wrist_state, extension_contact_func)
            
        def lift_contact_func(effort, av_effort):
            single_effort_threshold = 20.0
            av_effort_threshold = 20.0
            
            if (effort <= single_effort_threshold):
                rospy.loginfo('Lift single effort less than single_effort_threshold: {0} <= {1}'.format(effort, single_effort_threshold))
            if (av_effort <= av_effort_threshold):
                rospy.loginfo('Lift average effort less than av_effort_threshold: {0} <= {1}'.format(av_effort, av_effort_threshold))
                
            return ((effort <= single_effort_threshold) or
                    (av_effort < av_effort_threshold))
        
        self.lift_down_contact_detector = ContactDetector(hm.get_lift_state, lift_contact_func)
        
        
    def publish_map_point_cloud(self):
        if self.merged_map is not None:
            max_height_point_cloud = self.merged_map.max_height_im.to_point_cloud()
            self.point_cloud_pub.publish(max_height_point_cloud)
            
            pub_voi = True
            if pub_voi:
                marker = self.merged_map.max_height_im.voi.get_ros_marker(duration=1000.0)
                self.voi_marker_pub.publish(marker)

                
    def publish_nav_plan_markers(self, line_segment_path, image_to_points_mat, clicked_frame_id):
        path_height_m = 0.2
        points = [np.matmul(image_to_points_mat, np.array([p[0], p[1], path_height_m, 1.0]))[:3] for p in line_segment_path]
        points = [[p[0], p[1], path_height_m] for p in points]
        self.publish_path_markers(points, clicked_frame_id)

    def publish_path_markers(self, points, points_frame_id):
        path_height_m = 0.2
        points = [[p[0], p[1], path_height_m] for p in points]
        if self.prev_nav_markers is not None:
            # delete previous markers
            for m in self.prev_nav_markers.markers:
                m.action = m.DELETE
            self.navigation_plan_markers_pub.publish(self.prev_nav_markers)
        nav_markers = MarkerArray()
        duration_s = 1 * 60 
        timestamp = rospy.Time.now()
        m = hr.create_line_strip(points, 0, points_frame_id, timestamp, rgba=[0.0, 1.0, 0.0, 1.0], line_width_m=0.05, duration_s=duration_s)
        nav_markers.markers.append(m)
        for i, p in enumerate(points):
            m = hr.create_sphere_marker(p, i+1, points_frame_id, timestamp, rgba=[1.0, 1.0, 1.0, 1.0], diameter_m=0.15, duration_s=duration_s)
            nav_markers.markers.append(m)
        self.navigation_plan_markers_pub.publish(nav_markers)
        self.prev_nav_markers = nav_markers


    def trigger_align_with_nearest_cliff_service_callback(self, request):        
        manip = mp.ManipulationView(self.tf2_buffer, self.debug_directory)
        manip.move_head(self.move_to_pose)
        manip.update(self.point_cloud, self.tf2_buffer)
        if self.debug_directory is not None:
            dirname = self.debug_directory + 'align_with_nearest_cliff/'
            # If the directory does not already exist, create it.
            if not os.path.exists(dirname):
                os.makedirs(dirname)
            filename = 'nearest_cliff_scan_' + hm.create_time_string()
            manip.save_scan(dirname + filename)
        else:
            rospy.loginfo('FunmapNode trigger_align_with_nearest_cliff_service_callback: No debug directory provided, so debugging data will not be saved.')
        p0, p1, normal = manip.get_nearest_cliff('odom', self.tf2_buffer)
        if normal is not None: 
            cliff_ang = np.arctan2(normal[1], normal[0])

            # Find the robot's current pose in the odom frame.
            xya, timestamp = self.get_robot_floor_pose_xya(floor_frame='odom')
            robot_ang = xya[2]
            align_arm_ang = robot_ang + (np.pi/2.0)

            # Find the angle that the robot should turn in order
            # to point toward the next waypoint.
            turn_ang = hm.angle_diff_rad(cliff_ang, align_arm_ang)

            # Command the robot to turn to point to the next
            # waypoint.
            at_goal = self.move_base.turn(turn_ang, publish_visualizations=True)
            if not at_goal:
                message_text = 'Failed to reach turn goal.'
                rospy.loginfo(message_text)
                success=False
                message=message_text
            else:
                success = True
                message = 'Aligned with the nearest edge.'
        else:
            success = False
            message = 'Failed to detect cliff.'
                
        return TriggerResponse(
            success=success,
            message=message
            )
    
    def joint_states_callback(self, joint_states):
        self.extension_contact_detector.update(joint_states, self.stop_the_robot_service)
        self.wrist_position = self.extension_contact_detector.get_position()

        self.lift_down_contact_detector.update(joint_states, self.stop_the_robot_service)
        self.lift_position = self.lift_down_contact_detector.get_position()

        
    def trigger_reach_until_contact_service_callback(self, request):
        manip = mp.ManipulationView(self.tf2_buffer, self.debug_directory)
        manip.move_head(self.move_to_pose)
        manip.update(self.point_cloud, self.tf2_buffer)
        if self.debug_directory is not None:
            dirname = self.debug_directory + 'reach_until_contact/'
            # If the directory does not already exist, create it.
            if not os.path.exists(dirname):
                os.makedirs(dirname)
            filename = 'reach_until_contact_' + hm.create_time_string()
            manip.save_scan(dirname + filename)
        else:
            rospy.loginfo('FunmapNode trigger_reach_until_contact_service_callback: No debug directory provided, so debugging data will not be saved.')

        if self.use_hook:
            tooltip_frame = 'link_hook'
        else:
            tooltip_frame = 'link_grasp_center'
        reach_m = manip.estimate_reach_to_contact_distance(tooltip_frame, self.tf2_buffer)
        rospy.loginfo('----------------')
        rospy.loginfo('reach_m = {0}'.format(reach_m))
        rospy.loginfo('----------------')

        # Be aggressive moving in observed freespace and cautious
        # moving toward a perceived obstacle or unknown region.
        success = False
        message = 'Unknown result.'
        if self.wrist_position is not None:
            # The current wrist position needs to be known in order
            # for a reach command to be sent.
            max_reach_target_m = 0.5
            if (reach_m is not None):
                reach_target_m = reach_m + self.wrist_position
            else:
                reach_target_m = None

            if (reach_target_m is None) or (reach_target_m > max_reach_target_m):
                # Either the observed reach target was too far for the
                # arm, in which case we assume that something strange
                # happened and reach cautiously over the full reach.

                # Or, a freespace reach was not observed, so reach
                # cautiously over the full reach.
                direction_sign = 1
                success, message = self.extension_contact_detector.move_until_contact('wrist_extension', max_reach_target_m, direction_sign, self.move_to_pose)
            else: 
                # A freespace region was observed. Agressively move to
                # within a safe distance of the expected obstacle.
                
                safety_margin_m = 0.02
                safe_target_m = reach_target_m - safety_margin_m
                if self.use_hook:
                    safe_target_m = safe_target_m + 0.03
                if safe_target_m > self.wrist_position: 
                    pose = {'wrist_extension' : safe_target_m}
                    self.move_to_pose(pose, async=False)

                # target depth within the surface
                target_depth_m = 0.08 
                in_contact_target_m = reach_target_m + target_depth_m

                direction_sign = 1
                success, message = self.extension_contact_detector.move_until_contact('wrist_extension', in_contact_target_m, direction_sign, self.move_to_pose)
               
        return TriggerResponse(
            success=success,
            message=message
            )


    def trigger_lower_until_contact_service_callback(self, request):
        direction_sign = -1
        lowest_allowed_m = 0.3
        success, message = self.lift_down_contact_detector.move_until_contact('joint_lift', lowest_allowed_m, direction_sign, self.move_to_pose)
        return TriggerResponse(
            success=success,
            message=message
            )

    
    def trigger_global_localization_service_callback(self, request):
        self.perform_head_scan(localize_only=True, global_localization=True)
        return TriggerResponse(
            success=True,
            message='Completed localization with scan.'
            )
    
    def trigger_local_localization_service_callback(self, request):
        self.perform_head_scan(localize_only=True, global_localization=False, fast_scan=True)
        return TriggerResponse(
            success=True,
            message='Completed localization with scan.'
            )
        
    def trigger_head_scan_service_callback(self, request):
        self.perform_head_scan()
        return TriggerResponse(
            success=True,
            message='Completed head scan.'
            )
    
    def trigger_drive_to_scan_service_callback(self, request):

        if self.merged_map is None:
            return TriggerResponse(
                success=False,
                message='No map exists yet, so unable to drive to a good scan spot.'
            )
            
        max_height_im = self.merged_map.max_height_im
        
        robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image(self.tf2_buffer)
        robot_xya_pix = [robot_xy_pix[0], robot_xy_pix[1], robot_ang_rad]
        robot_x_pix = int(round(robot_xy_pix[0]))
        robot_y_pix = int(round(robot_xy_pix[1]))
        
        # Define the target maximum observation distance for any
        # observed point in the map. This serves as a goal for mapping.
        max_scan_distance_m = 1.5
        
        # The best case minimum width of the robot in meters when moving forward and backward. 
        min_robot_width_m = 0.34
        
        camera_height_m = 1.12

        floor_mask = sm.compute_floor_mask(max_height_im)
        
        # Select the next location on the map from which to
        # attempt to make a head scan.
        best_xy = na.select_next_scan_location(floor_mask, max_height_im, min_robot_width_m,
                                               robot_x_pix, robot_y_pix, robot_ang_rad,
                                               camera_height_m, max_scan_distance_m,
                                               display_on=False)
        if best_xy is None:
            return TriggerResponse(
                success=False,
                message='No good scan location was detected.'
            )
        
        # Plan an optimistic path on the floor to the next
        # location for scanning.
        end_xy = np.array(best_xy)

        success, message = self.navigate_to_map_pixel(end_xy, robot_xya_pix=robot_xya_pix, floor_mask=floor_mask)

        return TriggerResponse(
            success=success, 
            message=message
        )
    
    def pose_to_map_pixel(self, pose_stamped):
        clicked_frame_id = pose_stamped.header.frame_id
        clicked_timestamp = pose_stamped.header.stamp
        clicked_point = pose_stamped.pose.position
            
        # Check if a map exists
        if self.merged_map is None:
            success = False
            message = 'No map exists yet, so unable to drive to a good scan spot.'
            rospy.logerr(message)
            return None
            
        max_height_im = self.merged_map.max_height_im
        map_frame_id = self.merged_map.max_height_im.voi.frame_id
        
        points_to_image_mat, pi_timestamp = max_height_im.get_points_to_image_mat(clicked_frame_id, self.tf2_buffer)
        #lookup_time=clicked_timestamp)

        if (points_to_image_mat is not None):
            c_x = clicked_point.x
            c_y = clicked_point.y
            c_z = clicked_point.z
            clicked_xyz = np.array([c_x, c_y, c_z, 1.0])
            clicked_image_pixel = np.matmul(points_to_image_mat, clicked_xyz)
            i_x, i_y, i_z = clicked_image_pixel[:3]
            rospy.loginfo('clicked_image_pixel =' + str(clicked_image_pixel))
            end_xy = np.int64(np.round(np.array([i_x, i_y])))
            rospy.loginfo('end_xy =' + str(end_xy))
            return end_xy

        return None
    
    
    def plan_a_path(self, end_xy_pix, robot_xya_pix=None, floor_mask=None):
        # Transform the robot's current estimated pose as represented
        # by TF2 to the map image. Currently, the estimated pose is
        # based on the transformation from the map frame to the
        # base_link frame, which is updated by odometry and
        # corrections based on matching head scans to the map.
        path = None
        
        # Check if a map exists
        if self.merged_map is None:
            message = 'No map exists yet, so unable to drive to a good scan spot.'
            return path, message

        max_height_im = self.merged_map.max_height_im
        if robot_xya_pix is None: 
            robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image(self.tf2_buffer)
            robot_xya_pix = [robot_xy_pix[0], robot_xy_pix[1], robot_ang_rad]

        max_height_im = self.merged_map.max_height_im
        line_segment_path, message = na.plan_a_path(max_height_im, robot_xya_pix,
                                                    end_xy_pix, floor_mask=floor_mask)
        return line_segment_path, message

    

    def plan_to_reach(self, reach_xyz_pix, robot_xya_pix=None, floor_mask=None):
        # This is intended to perform coarse positioning of the
        # gripper near a target 3D point.
        robot_reach_xya_pix = None
        wrist_extension_m = None

        i_x, i_y, i_z = reach_xyz_pix
        
        max_height_im = self.merged_map.max_height_im
        # Check if a map exists
        if self.merged_map is None:
            message = 'No map exists yet, so unable to plan a reach.'
            rospy.logerr(message)
            return None, None
        
        if robot_xya_pix is None: 
            robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image(self.tf2_buffer)
            robot_xya_pix = [robot_xy_pix[0], robot_xy_pix[1], robot_ang_rad]
        
        end_xy_pix = np.int64(np.round(np.array([i_x, i_y])))
        m_per_height_unit = max_height_im.m_per_height_unit
        # move the gripper to be above the target point
        extra_target_height_m = 0.01
        target_z = i_z + (extra_target_height_m / m_per_height_unit)
        target_z_m = target_z * m_per_height_unit
        target_xyz_pix = (end_xy_pix[0], end_xy_pix[1], target_z)

        image_display_on = False

        manipulation_planner = mp.ManipulationPlanner()
        base_x_pix, base_y_pix, base_ang_rad, wrist_extension_m = manipulation_planner.base_pose(max_height_im,
                                                                                                 target_xyz_pix,
                                                                                                 robot_xya_pix,
                                                                                                 image_display_on=image_display_on)
        if image_display_on: 
            c = cv2.waitKey(0)
            
        if base_x_pix is None:
            rospy.logerr('No valid base pose found for reaching the target.')
            return None, None

        robot_reach_xya_pix = [base_x_pix, base_y_pix, base_ang_rad]

        base_link_point = max_height_im.get_pix_in_frame(np.array(reach_xyz_pix), 'base_link', self.tf2_buffer)
        
        simple_reach_plan = []
        
        # close the gripper
        simple_reach_plan.append({'joint_gripper_finger_left': 0.0})

        # move the lift to be at the height of the target
        # The fingers of the gripper touch the floor at a joint_lift
        # height of 0.0 m, so moving the lift link to the height of
        # the target will result in the fingers being at the height of
        # the target.
        height_m = base_link_point[2]
        safety_z_m = 0.0
        simple_reach_plan.append({'joint_lift': height_m + safety_z_m})
        
        # rotate the gripper to be in the center
        # of the swept volume of the wrist (a
        # little right of center when looking out
        # from the robot to the gripper)
        #simple_reach_plan.append({'joint_gripper': -0.25})
        simple_reach_plan.append({'joint_wrist_yaw': -0.25})
        
        # reach out to the target
        # Reach to a point that is not fully at the target.
        safety_reach_m = 0.1 # 10cm away from the target
        simple_reach_plan.append({'wrist_extension': wrist_extension_m - safety_reach_m})
        
        return robot_reach_xya_pix, simple_reach_plan
    

    def reach_to_click_callback(self, clicked_msg):
        rospy.loginfo('clicked_msg =' + str(clicked_msg))
            
        clicked_frame_id = clicked_msg.header.frame_id
        clicked_timestamp = clicked_msg.header.stamp
        clicked_point = clicked_msg.point

        max_height_im = self.merged_map.max_height_im
        # Check if a map exists
        if self.merged_map is None:
            message = 'No map exists yet, so unable to plan a reach.'
            rospy.logerr(message)
            return

        points_to_image_mat, pi_timestamp = max_height_im.get_points_to_image_mat(clicked_frame_id, self.tf2_buffer)

        if points_to_image_mat is None:
            rospy.logerr('points_to_image_mat not found')
            return

        c_x = clicked_point.x
        c_y = clicked_point.y
        c_z = clicked_point.z
        clicked_xyz = np.array([c_x, c_y, c_z, 1.0])
        clicked_image_pixel = np.matmul(points_to_image_mat, clicked_xyz)[:3]
        i_x, i_y, i_z = clicked_image_pixel
        rospy.loginfo('clicked_image_pixel =' + str(clicked_image_pixel))

        h, w = max_height_im.image.shape
        if not ((i_x >= 0) and (i_y >= 0) and (i_x < w) and (i_y < h)):
            rospy.logerr('clicked point does not fall within the bounds of the max_height_image')
            return 

        robot_xy_pix, robot_ang_rad, timestamp = max_height_im.get_robot_pose_in_image(self.tf2_buffer)
        robot_xya_pix = [robot_xy_pix[0], robot_xy_pix[1], robot_ang_rad]
        
        reach_xyz_pix = clicked_image_pixel
        robot_reach_xya_pix, simple_reach_plan = self.plan_to_reach(reach_xyz_pix, robot_xya_pix=robot_xya_pix)

        success, message = self.navigate_to_map_pixel(robot_reach_xya_pix[:2],
                                                      end_angle=robot_reach_xya_pix[2],
                                                      robot_xya_pix=robot_xya_pix)

        if success: 
            for pose in simple_reach_plan:
                self.move_to_pose(pose)
        else:
            rospy.logerr(message)
            rospy.logerr('Aborting reach attempt due to failed navigation')

        return
        
    
    def navigate_to_map_pixel(self, end_xy, end_angle=None, robot_xya_pix=None, floor_mask=None):
        # Set the D435i to Default mode for obstacle detection
        trigger_request = TriggerRequest() 
        trigger_result = self.trigger_d435i_default_mode_service(trigger_request)
        rospy.loginfo('trigger_result = {0}'.format(trigger_result))

        # Move the head to a pose from which the D435i can detect
        # obstacles near the front of the mobile base while moving
        # forward.
        self.move_base.head_to_forward_motion_pose()

        line_segment_path, message = self.plan_a_path(end_xy, robot_xya_pix=robot_xya_pix, floor_mask=floor_mask)
        if line_segment_path is None:
            success = False
            return success, message

        # Existence of the merged map is checked by plan_a_path, but
        # to avoid future issues I'm introducing this redundancy.
        if self.merged_map is None:
            success = False
            return success, 'No map available for planning and navigation.'
        max_height_im = self.merged_map.max_height_im
        map_frame_id = self.merged_map.max_height_im.voi.frame_id
                
        # Query TF2 to obtain the current estimated transformation
        # from the map image to the map frame.
        image_to_points_mat, ip_timestamp = max_height_im.get_image_to_points_mat(map_frame_id, self.tf2_buffer)
        
        if image_to_points_mat is not None:                

            # Publish a marker array to visualize the line segment path.
            self.publish_nav_plan_markers(line_segment_path, image_to_points_mat, map_frame_id)

            # Iterate through the vertices of the line segment path,
            # commanding the robot to drive to them in sequence using
            # in place rotations and forward motions.
            successful = True
            for p0, p1 in zip(line_segment_path, line_segment_path[1:]):
                # Query TF2 to obtain the current estimated transformation
                # from the image to the odometry frame.
                image_to_odom_mat, io_timestamp = max_height_im.get_image_to_points_mat('odom', self.tf2_buffer)

                # Query TF2 to obtain the current estimated transformation
                # from the robot's base_link frame to the odometry frame.
                robot_to_odom_mat, ro_timestamp = hm.get_p1_to_p2_matrix('base_link', 'odom', self.tf2_buffer)

                # Navigation planning is performed with respect to a
                # odom frame height of 0.0, so the heights of
                # transformed points are 0.0. The simple method of
                # handling the heights below assumes that the odom
                # frame is aligned with the floor, so that ignoring
                # the z coordinate is approximately equivalent to
                # projecting a point onto the floor.
                
                # Convert the current and next waypoints from map
                # image pixel coordinates to the odom
                # frame. 
                p0 = np.array([p0[0], p0[1], 0.0, 1.0])
                p0 = np.matmul(image_to_odom_mat, p0)[:2]
                p1 = np.array([p1[0], p1[1], 0.0, 1.0])
                next_point_xyz = np.matmul(image_to_odom_mat, p1)
                p1 = next_point_xyz[:2]

                # Find the robot's current pose in the odom frame.
                xya, timestamp = self.get_robot_floor_pose_xya()
                r0 = xya[:2]
                r_ang = xya[2]
                
                # Check how far the robot's current location is from
                # its current waypoint. The current waypoint is where
                # the robot would ideally be located.
                waypoint_tolerance_m = 0.25
                waypoint_error = np.linalg.norm(p0 - r0)
                rospy.loginfo('waypoint_error =' + str(waypoint_error))
                if waypoint_error > waypoint_tolerance_m:
                    message_text = 'Failed due to waypoint_error being above the maximum allowed error.'
                    rospy.loginfo(message_text)
                    success=False
                    message=message_text
                    return success, message

                # Find the angle in the odometry frame that would
                # result in the robot pointing at the next waypoint.
                travel_vector = p1 - r0
                travel_dist = np.linalg.norm(travel_vector)
                travel_ang = np.arctan2(travel_vector[1], travel_vector[0])
                rospy.loginfo('travel_dist =' + str(travel_dist))
                rospy.loginfo('travel_ang =' + str(travel_ang * (180.0/np.pi)))

                # Find the angle that the robot should turn in order
                # to point toward the next waypoint.
                turn_ang = hm.angle_diff_rad(travel_ang, r_ang)

                # Command the robot to turn to point to the next
                # waypoint.
                rospy.loginfo('robot turn angle in degrees =' + str(turn_ang * (180.0/np.pi)))
                at_goal = self.move_base.turn(turn_ang, publish_visualizations=True)
                if not at_goal:
                    message_text = 'Failed to reach turn goal.'
                    rospy.loginfo(message_text)
                    success=False
                    message=message_text
                    return success, message
                    
                # The head seems to drift sometimes over time, such
                # that the obstacle detection region is no longer
                # observed resulting in false positives. Hopefully,
                # this will correct the situation.
                self.move_base.head_to_forward_motion_pose()

                # FOR FUTURE DEVELOPMENT OF LOCAL NAVIGATION
                testing_future_code = False
                if testing_future_code: 
                    check_result = self.move_base.check_line_path(next_point_xyz, 'odom')
                    rospy.loginfo('Result of check line path = {0}'.format(check_result))
                    local_path, local_path_frame_id = self.move_base.local_plan(next_point_xyz, 'odom')
                    if local_path is not None:
                        rospy.loginfo('Found local path! Publishing markers for it!')
                        self.publish_path_markers(local_path, local_path_frame_id)
                    else:
                        rospy.loginfo('Did not find a local path...')
                
                # Command the robot to move forward to the next waypoing. 
                at_goal = self.move_base.forward(travel_dist, publish_visualizations=True)
                if not at_goal:
                    message_text = 'Failed to reach forward motion goal.'
                    rospy.loginfo(message_text)
                    success=False
                    message=message_text
                    return success, message
                
                rospy.loginfo('Turn and forward motion succeeded.')

            if end_angle is not None:
                # If a final target angle has been provided, rotate
                # the robot to match the target angle.
                rospy.loginfo('Attempting to achieve the final target orientation.')
                
                # Find the robot's current pose in the map frame. This
                # assumes that the target angle has been specified
                # with respect to the map frame.
                xya, timestamp = self.get_robot_floor_pose_xya(floor_frame='map')
                r_ang = xya[2]
            
                # Find the angle that the robot should turn in order
                # to point toward the next waypoint.
                turn_ang = hm.angle_diff_rad(end_angle, r_ang)

                # Command the robot to turn to point to the next
                # waypoint.
                rospy.loginfo('robot turn angle in degrees =' + str(turn_ang * (180.0/np.pi)))
                at_goal = self.move_base.turn(turn_ang, publish_visualizations=True)
                if not at_goal:
                    message_text = 'Failed to reach turn goal.'
                    rospy.loginfo(message_text)
                    success=False
                    message=message_text
                    return success, message
            
        success=True
        message='Completed drive to new scan location.'
        return success, message

    
    def perform_head_scan(self, fill_in_blindspot_with_second_scan=True, localize_only=False, global_localization=False, fast_scan=False):
        node = self
        
        trigger_request = TriggerRequest() 
        trigger_result = self.trigger_d435i_high_accuracy_mode_service(trigger_request)
        rospy.loginfo('trigger_result = {0}'.format(trigger_result))
            
        # Reduce the occlusion due to the arm and grabber. This is
        # intended to be run when the standard grabber is not holding
        # an object.
        ma.stow_and_lower_arm(node)

        # Create and perform a new full scan of the environment using
        # the head.
        head_scan = ma.HeadScan(voi_side_m=16.0)
        head_scan.execute_full(node, fast_scan=fast_scan)

        scaled_scan = None
        scaled_merged_map = None

        # Save the new head scan to disk.
        if self.debug_directory is not None:
            dirname = self.debug_directory + 'head_scans/'
            # If the directory does not already exist, create it.
            if not os.path.exists(dirname):
                os.makedirs(dirname)
            filename = 'head_scan_' + hm.create_time_string()
            head_scan.save(dirname + filename)
        else:
            rospy.loginfo('FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.')

        head_scan.make_robot_footprint_unobserved()
        save_merged_map = False
        
        if self.merged_map is None:
            # The robot does not currently have a map, so initialize
            # the map with the new head scan.
            rospy.loginfo('perform_head_scan: No map available, so setting the map to be the scan that was just taken.')
            self.merged_map = head_scan
            robot_pose = [head_scan.robot_xy_pix[0], head_scan.robot_xy_pix[1], head_scan.robot_ang_rad]
            self.robot_poses.append(robot_pose)
            self.localized = True
            save_merged_map = True
        else:
            if localize_only and (not global_localization):
                # The scan was performed to localize the robot locally.
                rospy.loginfo('perform_head_scan: Performing local localization.')
                use_full_size_scans = False
                if use_full_size_scans: 
                    affine_matrix, original_robot_map_pose, corrected_robot_map_pose = mm.estimate_scan_1_to_scan_2_transform(head_scan,
                                                                                                                              self.merged_map, 
                                                                                                                              display_on=False,
                                                                                                                              show_unaligned=False,
                                                                                                                              full_localization=False,
                                                                                                                              init_target=None,
                                                                                                                              grid_search=False,
                                                                                                                              small_search=False)
                else: 
                    original_robot_map_frame_pose, corrected_robot_map_frame_pose, original_robot_map_image_pose, corrected_robot_map_image_pose, scaled_scan, scaled_merged_map = ma.localize_with_reduced_images(head_scan, self.merged_map, global_localization=False, divisor=2, small_search=True)

                    corrected_robot_map_pose = corrected_robot_map_frame_pose
                    original_robot_map_pose = original_robot_map_frame_pose
                    # Save the scaled scans to disk for debugging.
                    if self.debug_directory is not None:
                        dirname = self.debug_directory + 'scaled_localization_scans/'
                        # If the directory does not already exist, create it.
                        if not os.path.exists(dirname):
                            os.makedirs(dirname)
                        time_string = hm.create_time_string()
                        filename = 'localization_scaled_head_scan_' + time_string
                        scaled_scan.save(dirname + filename)
                        filename = 'localization_scaled_merged_map_' + time_string
                        scaled_merged_map.save(dirname + filename)
                    else:
                        rospy.loginfo('FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.')
                self.localized = True
            elif (not self.localized) or (localize_only and global_localization):
                # The robot has not been localized with respect to the
                # current map or the scan was performed solely to
                # globally localize the robot. This attempts to
                # localize the robot on the map by reducing the sizes
                # of the scan and the map in order to more efficiently
                # search for a match globally.

                # This does not merge the new scan into the current map.
                rospy.loginfo('perform_head_scan: Performing global localization.')
                save_merged_map = False
                
                original_robot_map_frame_pose, corrected_robot_map_frame_pose, original_robot_map_image_pose, corrected_robot_map_image_pose, scaled_scan, scaled_merged_map = ma.localize_with_reduced_images(head_scan, self.merged_map, global_localization=True, divisor=6) #4)
                corrected_robot_map_pose = corrected_robot_map_frame_pose
                original_robot_map_pose = original_robot_map_frame_pose
                self.localized = True

                # Save the scaled scans to disk for debugging.
                if self.debug_directory is not None:
                    dirname = self.debug_directory + 'scaled_localization_scans/'
                    # If the directory does not already exist, create it.
                    if not os.path.exists(dirname):
                        os.makedirs(dirname)
                    time_string = hm.create_time_string()
                    filename = 'localization_scaled_head_scan_' + time_string
                    scaled_scan.save(dirname + filename)
                    filename = 'localization_scaled_merged_map_' + time_string
                    scaled_merged_map.save(dirname + filename)
                else:
                    rospy.loginfo('FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.')
            else: 
                # The robot has been localized with respect to the
                # current map, so proceed to merge the new head scan
                # into the map. This assumes that the robot's
                # estimated pose is close to its actual pose in the
                # map. It constrains the matching optimization to a
                # limited range of positions and orientations.
                rospy.loginfo('perform_head_scan: Performing local map merge.')
                original_robot_map_pose, corrected_robot_map_pose = mm.merge_scan_1_into_scan_2(head_scan, self.merged_map)
                save_merged_map = True
                
            # Store the corrected robot pose relative to the map frame.
            self.robot_poses.append(corrected_robot_map_pose)

            self.correct_robot_pose(original_robot_map_pose, corrected_robot_map_pose)
            pub_robot_markers = True
            if pub_robot_markers: 
                self.publish_corrected_robot_pose_markers(original_robot_map_pose, corrected_robot_map_pose)
                
        if save_merged_map: 
            # If the merged map has been updated, save it to disk.
            if self.debug_directory is not None:
                head_scans_dirname = self.debug_directory + 'head_scans/'
                # If the directory does not already exist, create it.
                if not os.path.exists(head_scans_dirname):
                    os.makedirs(head_scans_dirname)
                merged_maps_dirname = self.debug_directory + 'merged_maps/'
                # If the directory does not already exist, create it.
                if not os.path.exists(merged_maps_dirname):
                    os.makedirs(merged_maps_dirname)
                time_string = hm.create_time_string()
                if scaled_scan is not None: 
                    filename = 'localization_scaled_head_scan_' + time_string
                    scaled_scan.save(head_scans_dirname + filename)
                if scaled_merged_map is not None: 
                    filename = 'localization_scaled_merged_map_' + time_string
                    scaled_merged_map.save(merged_maps_dirname + filename)
                filename = 'merged_map_' + hm.create_time_string()
                self.merged_map.save(merged_maps_dirname + filename)
            else:
                rospy.loginfo('FunmapNode perform_head_scan: No debug directory provided, so debugging data will not be saved.')


        if fill_in_blindspot_with_second_scan and (not localize_only):
            # Turn the robot to the left in attempt to fill in its
            # blindspot due to its mast.
            turn_ang = (70.0/180.0) * np.pi
            
            # Command the robot to turn to point to the next
            # waypoint.
            rospy.loginfo('robot turn angle in degrees =' + str(turn_ang * (180.0/np.pi)))
            at_goal = self.move_base.turn(turn_ang, publish_visualizations=True)
            if not at_goal:
                message_text = 'Failed to reach turn goal.'
                rospy.loginfo(message_text)
            self.perform_head_scan(fill_in_blindspot_with_second_scan=False)


    def get_plan_service_callback(self, request):
        # request.start, request.goal, request.tolerance
        goal_pose = request.goal
        end_xy = self.pose_to_map_pixel(goal_pose)
        if end_xy is None:
            message = 'Failed to convert pose to map pixel.'
            rospy.logerr(message)
            return
        path, message = self.plan_a_path(end_xy)
        plan = Path()
        header = plan.header
        time_stamp = rospy.Time.now()
        header.stamp = time_stamp
        header.frame_id = 'map'
        if path is None:
            rospy.logerr(message)
            return plan

        # Existence of the merged map is checked by plan_a_path, but
        # to avoid future issues I'm introducing this redundancy.
        if self.merged_map is None:
            success = False
            return success, 'No map available for planning and navigation.'
        max_height_im = self.merged_map.max_height_im
        map_frame_id = self.merged_map.max_height_im.voi.frame_id
        
        # Query TF2 to obtain the current estimated transformation
        # from the map image to the map frame.
        image_to_points_mat, ip_timestamp = max_height_im.get_image_to_points_mat(map_frame_id, self.tf2_buffer)
        
        if image_to_points_mat is None:
            rospy.logerr('image_to_points_mat unavailable via TF2')
            return plan

        path_height_m = 0.0
        for xyz in path:
            image_point = np.array([xyz[0], xyz[1], 0.0, 1.0])
            map_point = np.matmul(image_to_points_mat, image_point)
            p = PoseStamped()
            p.header.frame_id = 'map'
            p.header.stamp = time_stamp
            p.pose.position.x = map_point[0]
            p.pose.position.y = map_point[1]
            p.pose.position.z = path_height_m
            plan.poses.append(p)
        return plan

    def correct_robot_pose(self, original_robot_map_pose_xya, corrected_robot_map_pose_xya):
        # Compute and broadcast the corrected transformation from
        # the map frame to the odom frame.
        print('original_robot_map_pose_xya =', original_robot_map_pose_xya)
        print('corrected_robot_map_pose_xya =', corrected_robot_map_pose_xya)
        x_delta = corrected_robot_map_pose_xya[0] - original_robot_map_pose_xya[0]
        y_delta = corrected_robot_map_pose_xya[1] - original_robot_map_pose_xya[1]
        ang_rad_correction = hm.angle_diff_rad(corrected_robot_map_pose_xya[2], original_robot_map_pose_xya[2])
        c = np.cos(ang_rad_correction)
        s = np.sin(ang_rad_correction)
        rot_mat = np.array([[c, -s], [s, c]])
        x_old, y_old, a_old = original_robot_map_pose_xya
        xy_old = np.array([x_old, y_old])
        tx, ty = np.matmul(rot_mat, -xy_old) + np.array([x_delta, y_delta]) + xy_old
        t = np.identity(4)
        t[0,3] = tx
        t[1,3] = ty
        t[:2,:2] = rot_mat
        self.map_to_odom_transform_mat = np.matmul(t, self.map_to_odom_transform_mat)
        self.tf2_broadcaster.sendTransform(create_map_to_odom_transform(self.map_to_odom_transform_mat))


    def publish_corrected_robot_pose_markers(self, original_robot_map_pose_xya, corrected_robot_map_pose_xya):
        # Publish markers to visualize the corrected and
        # uncorrected robot poses on the map.
        timestamp = rospy.Time.now()
        markers = MarkerArray()
        ang_rad = corrected_robot_map_pose_xya[2]
        x_axis = [np.cos(ang_rad), np.sin(ang_rad), 0.0]
        x, y, a = corrected_robot_map_pose_xya
        point = [x, y, 0.1]
        rgba = [0.0, 1.0, 0.0, 0.5]
        m_id = 0
        m = hr.create_sphere_marker(point, m_id, 'map', timestamp, rgba=rgba, diameter_m=0.1, duration_s=0.0)
        markers.markers.append(m)
        m_id += 1
        m = hr.create_axis_marker(point, x_axis, m_id, 'map', timestamp, rgba, length=0.2, arrow_scale=3.0)
        markers.markers.append(m)
        m_id += 1
        x, y, a = original_robot_map_pose_xya
        point = [x, y, 0.1]
        rgba = [1.0, 0.0, 0.0, 0.5]
        m = hr.create_sphere_marker(point, m_id, 'map', timestamp, rgba=rgba, diameter_m=0.1, duration_s=0.0)
        markers.markers.append(m)
        m_id += 1
        m = hr.create_axis_marker(point, x_axis, m_id, 'map', timestamp, rgba, length=0.2, arrow_scale=3.0)
        markers.markers.append(m)
        m_id += 1
        self.marker_array_pub.publish(markers)

        
    def set_robot_pose_callback(self, pose_with_cov_stamped):
        rospy.loginfo('Set robot pose called. This will set the pose of the robot on the map.')
        rospy.loginfo(pose_with_cov_stamped)

        original_robot_map_pose_xya, timestamp = self.get_robot_floor_pose_xya(floor_frame='map')

        pwcs = pose_with_cov_stamped
        frame_id = pwcs.header.frame_id
        timestamp = pwcs.header.stamp
        pose = pwcs.pose.pose
        
        if frame_id != 'map':
            lookup_time = rospy.Time(0) # return most recent transform
            timeout_ros = rospy.Duration(0.1)
            stamped_transform =  tf2_buffer.lookup_transform('map', frame_id, lookup_time, timeout_ros)
            map_pose = do_transform_pose(pose, stamped_transform)
        else:
            map_pose = pose
            
        p = map_pose.position
        q = map_pose.orientation
        q_list = [q.x, q.y, q.z, q.w]
        x = p.x
        y = p.y
        z = p.z
        roll, pitch, yaw = euler_from_quaternion(q_list)
        corrected_robot_map_pose_xya = [x, y, yaw]
        
        self.correct_robot_pose(original_robot_map_pose_xya, corrected_robot_map_pose_xya)
        self.publish_corrected_robot_pose_markers(original_robot_map_pose_xya, corrected_robot_map_pose_xya)


    def navigate_to_goal_topic_callback(self, goal_pose):
        rospy.loginfo('Navigate to goal simple navigate to goal topic received a command!')
        rospy.loginfo(goal_pose)

        end_xy = self.pose_to_map_pixel(goal_pose)
        if end_xy is None:
            message = 'Failed to convert pose to map pixel.'
            rospy.logerr(message)
            return
        success, message = self.navigate_to_map_pixel(end_xy)
        
        if success:
            rospy.loginfo(message)
        else:
            rospy.logerr(message)
        return

    
    def navigate_to_goal_action_callback(self, goal):
        # geometry_msgs/PoseStamped target_pose
        goal_pose = goal.target_pose
        rospy.loginfo('Navigate to goal simple action server received a command!')
        rospy.loginfo(goal_pose)

        end_xy = self.pose_to_map_pixel(goal_pose)
        if end_xy is None:
            message = 'Failed to convert pose to map pixel.'
            rospy.logerr(message)
            self.navigate_to_goal_action_server.set_aborted()
            return
        success, message = self.navigate_to_map_pixel(end_xy)
        
        if success: 
            result = MoveBaseResult()
            self.navigate_to_goal_action_server.set_succeeded(result)
        else:
            rospy.logerr(message)
            self.navigate_to_goal_action_server.set_aborted()
        return

        
    def main(self):
        hm.HelloNode.main(self, 'funmap', 'funmap')

        self.debug_directory = rospy.get_param('~debug_directory')
        
        self.merged_map = None
        self.localized = False

        if self.map_filename is not None:
            self.merged_map = ma.HeadScan.from_file(self.map_filename)
            self.localized = False
            
        ###########################
        # Related to move_base API 
        self.navigate_to_goal_action_server = actionlib.SimpleActionServer('/move_base',
                                                                           MoveBaseAction,
                                                                           execute_cb = self.navigate_to_goal_action_callback,
                                                                           auto_start = False)
        self.navigate_to_goal_action_server.start()

        self.navigation_goal_subscriber = rospy.Subscriber('/move_base_simple/goal',
                                                           PoseStamped,
                                                           self.navigate_to_goal_topic_callback)

        self.set_robot_pose_subscriber = rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, self.set_robot_pose_callback)
        
        self.get_plan_service = rospy.Service('/make_plan',
                                              GetPlan,
                                              self.get_plan_service_callback)
        ###########################
            
        self.trigger_head_scan_service = rospy.Service('/funmap/trigger_head_scan',
                                                       Trigger,
                                                       self.trigger_head_scan_service_callback)
        self.trigger_drive_to_scan_service = rospy.Service('/funmap/trigger_drive_to_scan',
                                                           Trigger,
                                                           self.trigger_drive_to_scan_service_callback)
        self.trigger_global_localization_service = rospy.Service('/funmap/trigger_global_localization',
                                                                Trigger,
                                                                self.trigger_global_localization_service_callback)
        self.trigger_local_localization_service = rospy.Service('/funmap/trigger_local_localization',
                                                                Trigger,
                                                                self.trigger_local_localization_service_callback)

        self.trigger_align_with_nearest_cliff_service = rospy.Service('/funmap/trigger_align_with_nearest_cliff',
                                                                     Trigger,
                                                                     self.trigger_align_with_nearest_cliff_service_callback)

        self.trigger_reach_until_contact_service = rospy.Service('/funmap/trigger_reach_until_contact',
                                                                 Trigger,
                                                                 self.trigger_reach_until_contact_service_callback)

        self.trigger_lower_until_contact_service = rospy.Service('/funmap/trigger_lower_until_contact',
                                                                 Trigger,
                                                                 self.trigger_lower_until_contact_service_callback)

        
        self.reach_to_click_subscriber = rospy.Subscriber('/clicked_point', PointStamped, self.reach_to_click_callback)

        
        default_service = '/camera/switch_to_default_mode'
        high_accuracy_service = '/camera/switch_to_high_accuracy_mode'
        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service)
        rospy.wait_for_service(default_service)
        rospy.loginfo('Node ' + self.node_name + ' connected to ' + default_service)
        self.trigger_d435i_default_mode_service = rospy.ServiceProxy(default_service, Trigger)
        rospy.wait_for_service(high_accuracy_service)
        rospy.loginfo('Node ' + self.node_name + ' connected to'  + high_accuracy_service)
        self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger)

        self.tf2_broadcaster = tf2_ros.TransformBroadcaster()

        self.point_cloud_pub = rospy.Publisher('/funmap/point_cloud2', PointCloud2, queue_size=1)
        self.voi_marker_pub = rospy.Publisher('/funmap/voi_marker', Marker, queue_size=1)
        self.marker_array_pub = rospy.Publisher('/funmap/marker_array', MarkerArray, queue_size=1)
        self.navigation_plan_markers_pub = rospy.Publisher('/funmap/navigation_plan_markers', MarkerArray, queue_size=1)
        self.obstacle_point_cloud_pub = rospy.Publisher('/funmap/obstacle_point_cloud2', PointCloud2, queue_size=1)

        self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
        
        self.rate = 5.0
        rate = rospy.Rate(self.rate)

        self.move_base = nv.MoveBase(self, self.debug_directory)
                
        self.map_to_odom_transform_mat = np.identity(4)
        while not rospy.is_shutdown():
            self.tf2_broadcaster.sendTransform(create_map_to_odom_transform(self.map_to_odom_transform_mat))
            self.publish_map_point_cloud()
            rate.sleep()
            
        
if __name__ == '__main__':
    try:
        parser = ap.ArgumentParser(description='Keyboard teleoperation for stretch.')
        parser.add_argument('--load_map', default=None, help='Provide directory from which to load a map.')
        args, unknown = parser.parse_known_args()
        map_filename = args.load_map if args.load_map else None
        node = FunmapNode(map_filename)
        node.main()
        rospy.spin()
    except KeyboardInterrupt:
        print('interrupt received, so shutting down')