diff --git a/velodyne_configuration/CMakeLists.txt b/velodyne_configuration/CMakeLists.txt index 7cd841b6..fbd4110d 100644 --- a/velodyne_configuration/CMakeLists.txt +++ b/velodyne_configuration/CMakeLists.txt @@ -26,5 +26,6 @@ include_directories( catkin_install_python(PROGRAMS nodes/configuration_node.py nodes/example_node.py + nodes/initialization_node.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) \ No newline at end of file diff --git a/velodyne_configuration/nodes/configuration_node.py b/velodyne_configuration/nodes/configuration_node.py index d0d97e64..7f43252e 100644 --- a/velodyne_configuration/nodes/configuration_node.py +++ b/velodyne_configuration/nodes/configuration_node.py @@ -35,22 +35,15 @@ def __init__(self): rospy.loginfo("Velodyne configuration node initializing") rospy.init_node("velodyne_configuration_node", anonymous=False) - ns = '/sensor/lidar/vls128_roof' - parameter_name = rospy.resolve_name('~ns_transform') - if rospy.has_param(parameter_name): - ns = rospy.get_param(parameter_name) - else: - rospy.loginfo("name space parameter not found, using default ip %s" % ns) + #ns = '/sensor/lidar/vls128_roof' + #parameter_name = rospy.resolve_name('~ns_transform') + #if rospy.has_param(parameter_name): + # ns = rospy.get_param(parameter_name) + #else: + # rospy.loginfo("name space parameter not found, using default ns %s" % ns) - self._set_service = rospy.Service('/set_configuration', VelodyneSetConfiguration, - self.set_configuration) - self._get_service = rospy.Service('/request_configuration', - VelodyneRequestConfiguration, - self.get_configuration) - self._cmd_service = rospy.Service('/special_command', VelodyneSpecialCommands, - self.send_command) - velodyne_ip = '192.168.3.201' + velodyne_ip = '0.0.0.1' parameter_name = rospy.resolve_name('~velodyne_ip') if rospy.has_param(parameter_name): velodyne_ip = rospy.get_param(parameter_name) @@ -65,12 +58,20 @@ def __init__(self): if rospy.has_param(parameter_name): self.snapshot_path = rospy.get_param(parameter_name) else: - rospy.loginfo("parameter_name parameter not found, using default ip %s" % self.snapshot_path) + rospy.loginfo("snapshot_path parameter not found, using default folder %s" % self.snapshot_path) print("Create configuration Object") self.configurator = Configurator(velodyne_ip) # Loads config object with current state print("Configuration node ready") + self._set_service = rospy.Service('set_configuration', VelodyneSetConfiguration, + self.set_configuration) + self._get_service = rospy.Service('request_configuration', + VelodyneRequestConfiguration, + self.get_configuration) + self._cmd_service = rospy.Service('special_commands', VelodyneSpecialCommands, + self.send_command) + def send_command(self, request): response = VelodyneSpecialCommandsResponse() try: @@ -189,18 +190,18 @@ def set_configuration(self, request): try: self.configurator.set_setting(k, changes[k]) except ValueError as e: - print("Caught ValueError Error while setting current configuration") + print("Caught ValueError Error while setting configuration") print(e.args) response.success = False return response except NameError as e: - print("Caught NameError Error while setting current configuration") + print("Caught NameError Error while setting configuration") print(e.args) response.success = False return response except Exception as e: - print("Caught Exception Error while setting current configuration") + print("Caught Exception Error while setting configuration") print(e.args) response.success = False return response diff --git a/velodyne_configuration/nodes/initialization_node.py b/velodyne_configuration/nodes/initialization_node.py index 19d7297e..9d7beec0 100644 --- a/velodyne_configuration/nodes/initialization_node.py +++ b/velodyne_configuration/nodes/initialization_node.py @@ -49,26 +49,19 @@ def request_config_from_current_config(current): if __name__ == "__main__": try: - rospy.init_node("velodyne_configuration_test_node", anonymous=True) + rospy.init_node("velodyne_initialization_node", anonymous=True) rospy.loginfo("Velodyne initialization node starting") - ns = '/sensor/lidar/vls128_roof' - parameter_name = rospy.resolve_name('~ns_transform') - if rospy.has_param(parameter_name): - ns = rospy.get_param(parameter_name) - else: - rospy.loginfo("name space parameter not found, using default ip %s" % ns) - # Define and wait for the services - set_config_srv_proxy = rospy.ServiceProxy('/set_configuration', + set_config_srv_proxy = rospy.ServiceProxy('set_configuration', VelodyneSetConfiguration) - request_config_srv_proxy = rospy.ServiceProxy('/request_configuration', + request_config_srv_proxy = rospy.ServiceProxy('request_configuration', VelodyneRequestConfiguration) - rospy.wait_for_service('/sensor/lidar/vls128_roof/set_configuration') - rospy.wait_for_service('/sensor/lidar/vls128_roof/request_configuration') - - rospy.loginfo("All Velodyne configuration services available") + rospy.loginfo("Velodyne Initialization node: Waiting for services to become available") + rospy.wait_for_service(set_config_srv_proxy.resolved_name) + rospy.wait_for_service(request_config_srv_proxy.resolved_name) + rospy.loginfo("Velodyne Initialization node: Services Found, Initializing") # Get the desired parameters from the launch file @@ -141,26 +134,32 @@ def request_config_from_current_config(current): rospy.logerr("The desired phase should be a float value") rospy.logerr(phase) raise TypeError - if not phase*100 < 10000: + if not phase*100 <= 99999: rospy.logerr("The maximum allowed precision of the phase parameter is 2 decimal digits") rospy.logerr(phase) raise ValueError else: rospy.loginfo("phase parameter not found, using default value %i" % phase) - # Request current configuration - get_config_request = VelodyneRequestConfigurationRequest() - get_config_request.stamp = rospy.Time.now() - current_config = request_config_srv_proxy(get_config_request) - set_config_request = request_config_from_current_config(current_config) + done = False + while not done: + # Request current configuration + get_config_request = VelodyneRequestConfigurationRequest() + get_config_request.stamp = rospy.Time.now() + current_config = request_config_srv_proxy(get_config_request) + set_config_request = request_config_from_current_config(current_config) + + set_config_request.returns.return_mode = return_mode + set_config_request.rpm = rpm + set_config_request.fov_start = fov_start + set_config_request.fov_end = fov_end + set_config_request.phaselock = phaselock + set_config_request.phase = int(phase*100) + set_response = set_config_srv_proxy(set_config_request) + done = set_response.success + + rospy.loginfo("Velodyne configuration successful, shutting down node") - set_config_request.returns.return_mode = return_mode - set_config_request.rpm = rpm - set_config_request.fov_start = fov_start - set_config_request.fov_end = fov_end - set_config_request.phaselock = phaselock - set_config_request.phase = int(phase*100) - set_response = set_config_srv_proxy(set_config_request) @@ -169,4 +168,4 @@ def request_config_from_current_config(current): print(inst.args) # arguments stored in .args print(inst) - rospy.spin() + diff --git a/velodyne_configuration/src/velodyne_curl_communication_lib/velodyne_curl_config.py b/velodyne_configuration/src/velodyne_curl_communication_lib/velodyne_curl_config.py index d7cec5cb..718def50 100644 --- a/velodyne_configuration/src/velodyne_curl_communication_lib/velodyne_curl_config.py +++ b/velodyne_configuration/src/velodyne_curl_communication_lib/velodyne_curl_config.py @@ -203,14 +203,16 @@ def test_connection(self): print(e) raise RuntimeError from e - print('Sensor laser is %b, motor rpm is %i' % (cfg.conf['laser'], cfg.conf['rpm'])) + print('Sensor laser is %r, motor rpm is %i' % (cfg.conf['laser'], cfg.conf['rpm'])) def _update_conf_from_snapshot(self): # Download a temporal snapshot to get the current state of the sensor url = VelodyneConfiguration.get_command_url("get_snapshot", self.Base_URL) with tempfile.NamedTemporaryFile() as temp_file: temp_file_path = temp_file.name + # ToDo Timeout urllib.request.urlretrieve(url, temp_file_path) + time.sleep(2.0) with open(temp_file_path, 'r') as file: son = json.load(file) @@ -257,6 +259,7 @@ def bool_to_on_off(self, b_val): return 'off' def set_setting(self, setting_id, value): + print("Settin %s to %r" % (setting_id, value)) if setting_id in VelodyneConfiguration.conf_ids: rc = None if setting_id == 'rpm': @@ -298,10 +301,14 @@ def set_setting(self, setting_id, value): elif setting_id == 'phase': current_phaselock = self.bool_to_on_off(self.config.conf["phaselock"]) + #print(current_phaselock) if VelodyneConfiguration.check_phase_lock_soll_val(value): + cmd = {"enabled": current_phaselock, "offset": str(value), "offsetInput": str(value / 100.0)} - rc = self.sensor_do(self.config.get_command_url("set_setting", self.Base_URL) + 'phaselock', + # print(cmd) + rc = self.sensor_do(self.config.get_command_url("set_setting", self.Base_URL) + '/phaselock', urlencode(cmd)) + # print("setted phase to desired value") if rc: self.config.conf[setting_id] = value @@ -311,11 +318,17 @@ def set_setting(self, setting_id, value): elif setting_id == 'phaselock': current_phase_value = self.config.conf["phase"] str_value = self.bool_to_on_off(value) + #print(current_phase_value) + #print(str_value) if VelodyneConfiguration.check_on_off_soll_val(str_value): cmd = {"enabled": str_value, "offset": str(current_phase_value), "offsetInput": str(current_phase_value / 100)} - rc = self.sensor_do(self.config.get_command_url("set_setting", self.Base_URL) + 'phaselock', + #print(cmd) + #print(self.config.get_command_url("set_setting", self.Base_URL) + 'phaselock') + #print(urlencode(cmd)) + rc = self.sensor_do(self.config.get_command_url("set_setting", self.Base_URL) + '/phaselock', urlencode(cmd)) + print("phaselock set to desired value") if rc: self.config.conf[setting_id] = value else: @@ -348,13 +361,11 @@ def set_setting(self, setting_id, value): self.config.conf[setting_id] = value self.safe_current_config_to_sensor() self.reset_sensor() - else: raise NameError("The passed setting is not recognised Name:%s " % setting_id) if not rc: raise Exception('Fail to set setting in the sensor Name:%s ' % setting_id) - else: raise NameError("Parameter name not found. Name:%s " % setting_id) @@ -452,8 +463,10 @@ def download_snapshot(self, folder_path, file_name=None): file_path = os.path.join(folder_path, file_name) try: + # Todo Time out urllib.request.urlretrieve(url, file_path) print(f'File downloaded and saved to {file_path}') + time.sleep(1.5) except Exception as e: print(f'Failed to download the file. Error: {e}') raise RuntimeError from e