From c535b29acea2881f2d23a69c89df36d532a3ffee Mon Sep 17 00:00:00 2001 From: tas Date: Wed, 17 Jul 2024 13:02:55 +0200 Subject: [PATCH] velodyne_configuration: Fix bugs in car. --- .../nodes/configuration_node.py | 30 +++++++------ velodyne_configuration/nodes/example_node.py | 42 +++++++++++-------- .../velodyne_curl_config.py | 20 ++++++--- .../srv/VelodyneRequestConfiguration.srv | 4 +- velodyne_msgs/srv/VelodyneSpecialCommands.srv | 1 + 5 files changed, 60 insertions(+), 37 deletions(-) diff --git a/velodyne_configuration/nodes/configuration_node.py b/velodyne_configuration/nodes/configuration_node.py index 683f6d2c..20b107a4 100644 --- a/velodyne_configuration/nodes/configuration_node.py +++ b/velodyne_configuration/nodes/configuration_node.py @@ -10,6 +10,7 @@ from velodyne_msgs.srv import VelodyneSetConfiguration from velodyne_msgs.srv import VelodyneSetConfigurationResponse from velodyne_msgs.srv import VelodyneSpecialCommands +from velodyne_msgs.srv import VelodyneSpecialCommandsRequest from velodyne_msgs.srv import VelodyneSpecialCommandsResponse import os @@ -36,7 +37,8 @@ def __init__(self): self._set_service = rospy.Service('/sensor/lidar/vls128_roof/set_configuration', VelodyneSetConfiguration, self.set_configuration) - self._get_service = rospy.Service('/sensor/lidar/vls128_roof/request_configuration', VelodyneRequestConfiguration, + self._get_service = rospy.Service('/sensor/lidar/vls128_roof/request_configuration', + VelodyneRequestConfiguration, self.get_configuration) self._cmd_service = rospy.Service('/sensor/lidar/vls128_roof/special_command', VelodyneSpecialCommands, self.send_command) @@ -58,12 +60,14 @@ def __init__(self): else: rospy.loginfo("parameter_name parameter not found, using default ip %s", self.snapshot_path) + print("Create configuration Object") self.configurator = Configurator(velodyne_ip) # Loads config object with current state + print("Configuration node ready") def send_command(self, request): response = VelodyneSpecialCommandsResponse() try: - if request.command == VelodyneSpecialCommands.DIAGNOSTICS: + if request.command == VelodyneSpecialCommandsRequest.DIAGNOSTICS: diag = self.configurator.get_diagnostics() response.all_in_range = True outside_range = [] @@ -77,13 +81,13 @@ def send_command(self, request): response.parameters = outside_range response.success = True - elif request.command == VelodyneSpecialCommands.RESET_SENSOR: + elif request.command == VelodyneSpecialCommandsRequest.RESET_SENSOR: self.configurator.reset_sensor() response.success = True - elif request.command == VelodyneSpecialCommands.DOWNLOAD_SNAPSHOT: + elif request.command == VelodyneSpecialCommandsRequest.DOWNLOAD_SNAPSHOT: self.configurator.download_snapshot(self.snapshot_path) response.success = True - elif request.command == VelodyneSpecialCommands.SAVE_CONFIG: + elif request.command == VelodyneSpecialCommandsRequest.SAVE_CONFIG: self.configurator.save_current_config_to_sensor() response.success = True else: @@ -96,14 +100,14 @@ def send_command(self, request): return response def get_configuration(self, request): - + print("Request to get current configuration") response = VelodyneRequestConfigurationResponse() try: - current_conf = self.configurator.get_current_configuration() + current_conf = self.configurator.get_current_configuration().conf response.gps_pps_state = current_conf["gps_pps_state"] response.gps_position = current_conf["gps_position"] - response.tod_time.sec = current_conf["tod_sec"] - response.tod_time.nanosec = current_conf["tod_nsec"] + response.tod_time.secs = current_conf["tod_sec"] + response.tod_time.nsecs = current_conf["tod_nsec"] response.usectoh = current_conf["usectoh"] response.motor_state = current_conf["motor_state"] response.rpm = current_conf["rpm"] @@ -122,8 +126,8 @@ def get_configuration(self, request): response.net_addr = current_conf["net_addr"] response.net_mask = current_conf["net_mask"] response.net_gateway = current_conf["net_gateway"] - response.net_dhcp = current_conf["net_dhcp"] - response.net_mac_addr = current_conf["net_mac_addr"] + #response.net_dhcp = current_conf["net_dhcp"] + #response.net_mac_addr = current_conf["net_mac_addr"] response.success = True except RuntimeError as e: @@ -137,7 +141,7 @@ def get_configuration(self, request): return response def set_configuration(self, request): - + print("Request to set configuration") # Find what is different changes = {} if request.rpm != self.configurator.get_setting("rpm"): @@ -147,7 +151,7 @@ def set_configuration(self, request): if request.fov_end != self.configurator.get_setting("fov_end"): changes["fov_end"] = request.fov_end if request.returns.return_mode != ConfiguratorNode.return_modes[self.configurator.get_setting("returns")]: - changes["returns"].return_mode = ConfiguratorNode.return_mode_to_str_cmd[request.returns.return_mode] + changes["returns"] = request.returns.return_mode if request.phaselock != self.configurator.get_setting("phaselock"): changes["phaselock"] = request.phaselock if request.phase != self.configurator.get_setting("phase"): diff --git a/velodyne_configuration/nodes/example_node.py b/velodyne_configuration/nodes/example_node.py index e6935fd3..83ab5c7b 100644 --- a/velodyne_configuration/nodes/example_node.py +++ b/velodyne_configuration/nodes/example_node.py @@ -7,13 +7,18 @@ from velodyne_msgs.msg import VelodyneReturnMode import os from velodyne_msgs.srv import VelodyneRequestConfiguration +from velodyne_msgs.srv import VelodyneRequestConfigurationRequest from velodyne_msgs.srv import VelodyneRequestConfigurationResponse from velodyne_msgs.srv import VelodyneSetConfiguration +from velodyne_msgs.srv import VelodyneSetConfigurationRequest from velodyne_msgs.srv import VelodyneSetConfigurationResponse from velodyne_msgs.srv import VelodyneSpecialCommands +from velodyne_msgs.srv import VelodyneSpecialCommandsRequest from velodyne_msgs.srv import VelodyneSpecialCommandsResponse +# TODO add wait for services + class ConfiguratorNode: def __init__(self): rospy.init_node("velodyne_configuration_test_node", anonymous=True) @@ -21,15 +26,19 @@ def __init__(self): self._set_config_srv_proxy = rospy.ServiceProxy('/sensor/lidar/vls128_roof/set_configuration', VelodyneSetConfiguration) - self._get_config_srv_proxy = rospy.ServiceProxy('/sensor/lidar/vls128_roof/request_configuration', - VelodyneRequestConfiguration) + self._request_config_srv_proxy = rospy.ServiceProxy('/sensor/lidar/vls128_roof/request_configuration', + VelodyneRequestConfiguration) self._special_config_srv_proxy = rospy.ServiceProxy('/sensor/lidar/vls128_roof/special_command', VelodyneSpecialCommands) # Get current conf and change the desired settings - get_config_request = VelodyneRequestConfiguration() + get_config_request = VelodyneRequestConfigurationRequest() get_config_request.stamp = rospy.Time.now() - current_config = self._get_config_srv_proxy (get_config_request) + print(get_config_request.stamp.secs) + print(get_config_request.stamp.nsecs) + print(get_config_request.stamp) + print(type(get_config_request)) + current_config = self._request_config_srv_proxy(get_config_request) if current_config.success: print("received current configuration") @@ -79,9 +88,9 @@ def __init__(self): set_config_request.returns.return_mode = VelodyneReturnMode.DUAL set_response = self._set_config_srv_proxy(set_config_request) - spc_cmd_config_request = VelodyneSpecialCommands() - spc_cmd_config_request.stamp = rospy.Time.now() - spc_cmd_config_request.command = VelodyneSpecialCommands.DIAGNOSTICS + spc_cmd_config_request = VelodyneSpecialCommandsRequest() + # spc_cmd_config_request.stamp = rospy.Time.now() + spc_cmd_config_request.command = VelodyneSpecialCommandsRequest.DIAGNOSTICS spc_cmd_response = self._special_config_srv_proxy(spc_cmd_config_request) @@ -92,23 +101,22 @@ def __init__(self): for p in spc_cmd_response.parameters: print(p) - spc_cmd_config_request = VelodyneSpecialCommands() - spc_cmd_config_request.stamp = rospy.Time.now() - spc_cmd_config_request.command = VelodyneSpecialCommands.DOWNLOAD_SNAPSHOT + spc_cmd_config_request = VelodyneSpecialCommandsRequest() + # spc_cmd_config_request.stamp = rospy.Time.now() + spc_cmd_config_request.command = VelodyneSpecialCommandsRequest.DOWNLOAD_SNAPSHOT spc_cmd_response = self._special_config_srv_proxy(spc_cmd_config_request) - - spc_cmd_config_request = VelodyneSpecialCommands() - spc_cmd_config_request.stamp = rospy.Time.now() - spc_cmd_config_request.command = VelodyneSpecialCommands.RESET_SENSOR + spc_cmd_config_request = VelodyneSpecialCommandsRequest() + # spc_cmd_config_request.stamp = rospy.Time.now() + spc_cmd_config_request.command = VelodyneSpecialCommandsRequest.RESET_SENSOR spc_cmd_response = self._special_config_srv_proxy(spc_cmd_config_request) # Get current conf and change the desired settings - get_config_request = VelodyneRequestConfiguration() + get_config_request = VelodyneRequestConfigurationRequest() get_config_request.stamp = rospy.Time.now() - current_config = self._get_config_srv_proxy (get_config_request) + current_config = self._request_config_srv_proxy(get_config_request) set_config_request = self.request_config_from_current_config(current_config) set_config_request.returns.return_mode = VelodyneReturnMode.STRONGEST set_config_request.rpm = 300 @@ -117,7 +125,7 @@ def __init__(self): set_response = self._set_config_srv_proxy(set_config_request) def request_config_from_current_config(self, current): - set_config_request = VelodyneSetConfiguration() + set_config_request = VelodyneSetConfigurationRequest() set_config_request.stamp = rospy.Time.now() set_config_request.rpm = current.rpm 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 a0ce41af..f7f11cd1 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 @@ -190,21 +190,26 @@ def __init__(self, velodyne_ip): self.Base_URL = 'http://' + velodyne_ip + '/cgi/' self.sensor = pycurl.Curl() self.buffer = BytesIO() + print("Testing connection to sensor") self.test_connection() + print("Connection OK") def test_connection(self): # Test connection to sensor try: - status = self.get_current_configuration() + cfg = self.get_current_configuration() + except RuntimeError as e: + print(e) raise RuntimeError from e - print('Sensor laser is %s, motor rpm is %s', - status['laser']['state'], status['motor']['rpm']) + + print('Sensor laser is %b, 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 urllib.request.urlretrieve(url, temp_file_path) @@ -357,7 +362,7 @@ def set_setting(self, setting_id, value): def get_setting(self, setting_id): try: - value = self.config.conf["setting_id"] + value = self.config.conf[setting_id] except KeyError: print(" %s is not a valid setting", setting_id) return None @@ -408,7 +413,9 @@ def check_diagnostics_parameter(self, t_b, diag_par, value): def get_current_configuration(self): try: + print("Get status") status = self._request_json('get_status') + print("Status received") except Exception as e: print(e) raise RuntimeError from e @@ -427,8 +434,9 @@ def get_current_configuration(self): self.config.conf["ns_per_rev"] = status["motor"]["ns_per_rev"] self.config.conf["laser"] = True if status["laser"]["state"] == "On" else False + print("Get Snapshot") self._update_conf_from_snapshot() - + print("Snapshot received") return self.config def download_snapshot(self, folder_path, file_name=None): diff --git a/velodyne_msgs/srv/VelodyneRequestConfiguration.srv b/velodyne_msgs/srv/VelodyneRequestConfiguration.srv index ab7e1d22..bc42f6f3 100644 --- a/velodyne_msgs/srv/VelodyneRequestConfiguration.srv +++ b/velodyne_msgs/srv/VelodyneRequestConfiguration.srv @@ -1,7 +1,7 @@ # request time stamp --- -#response +# response time stamp bool success string gps_pps_state @@ -23,5 +23,7 @@ string host_tport string net_addr string net_mask string net_gateway +string net_dhcp +string net_mac_addr diff --git a/velodyne_msgs/srv/VelodyneSpecialCommands.srv b/velodyne_msgs/srv/VelodyneSpecialCommands.srv index 20975d14..e94141fa 100644 --- a/velodyne_msgs/srv/VelodyneSpecialCommands.srv +++ b/velodyne_msgs/srv/VelodyneSpecialCommands.srv @@ -4,6 +4,7 @@ uint8 RESET_SENSOR = 1 uint8 DOWNLOAD_SNAPSHOT = 2 uint8 SAVE_CONFIG = 3 +time stamp uint8 command ---