velodyne_configuration: Fix bugs in car.

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"):

velodyne_configuration/nodes/example_node.py

@@ -7,29 +7,38 @@
 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)
         rospy.loginfo("Velodyne configuration test node initializing")
 
         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

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):

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
 
 

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
 
 ---