You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
82 lines
3.1 KiB
82 lines
3.1 KiB
#!/usr/bin/env python
|
|
|
|
from __future__ import print_function
|
|
|
|
import rospy
|
|
import dynamic_reconfigure.client
|
|
from std_srvs.srv import Trigger, TriggerResponse
|
|
import threading
|
|
|
|
class D435iConfigureNode:
|
|
def __init__(self):
|
|
self.rate = 1.0
|
|
self.visual_preset = None
|
|
self.mode_lock = threading.Lock()
|
|
|
|
def turn_on_default_mode(self):
|
|
with self.mode_lock:
|
|
self.locked_mode_id = 1
|
|
self.locked_mode_name = 'Default'
|
|
self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name})
|
|
rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name))
|
|
|
|
def turn_on_high_accuracy_mode(self):
|
|
with self.mode_lock:
|
|
self.locked_mode_id = 3
|
|
self.locked_mode_name = 'High Accuracy'
|
|
self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name})
|
|
rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name))
|
|
|
|
def default_mode_service_callback(self, request):
|
|
self.turn_on_default_mode()
|
|
return TriggerResponse(
|
|
success=True,
|
|
message='Default mode enabled.'
|
|
)
|
|
|
|
def high_accuracy_mode_service_callback(self, request):
|
|
self.turn_on_high_accuracy_mode()
|
|
return TriggerResponse(
|
|
success=True,
|
|
message='High Accuracy mode enabled.'
|
|
)
|
|
|
|
def main(self):
|
|
rospy.sleep(3) # sleep for 3 seconds to wait for D435i to be ready
|
|
rospy.init_node('configure_d435i')
|
|
self.node_name = rospy.get_name()
|
|
rospy.loginfo("{0} started".format(self.node_name))
|
|
self.parameter_client = dynamic_reconfigure.client.Client('/camera/stereo_module/')
|
|
|
|
self.switch_to_manipulation_mode_service = rospy.Service('/camera/switch_to_default_mode',
|
|
Trigger,
|
|
self.default_mode_service_callback)
|
|
|
|
self.switch_to_navigation_mode_service = rospy.Service('/camera/switch_to_high_accuracy_mode',
|
|
Trigger,
|
|
self.high_accuracy_mode_service_callback)
|
|
|
|
initial_mode = rospy.get_param('~initial_mode')
|
|
|
|
rospy.loginfo("initial_mode = {0}".format(initial_mode))
|
|
|
|
if initial_mode == 'High Accuracy':
|
|
self.turn_on_high_accuracy_mode()
|
|
elif initial_mode == 'Default':
|
|
self.turn_on_default_mode()
|
|
else:
|
|
error_string = 'initial_mode = {0} not recognized. Setting to D435i to Default mode.'.format(initial_mode)
|
|
rospy.logerr(error_string)
|
|
self.turn_on_default_mode()
|
|
|
|
rate = rospy.Rate(self.rate)
|
|
while not rospy.is_shutdown():
|
|
rate.sleep()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
node = D435iConfigureNode()
|
|
node.main()
|
|
except KeyboardInterrupt:
|
|
print('interrupt received, so shutting down')
|