velodyne_configuration: Add initialization mode to Cmake. Change serv…

…ice names so that they work with the nodes namespace.

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

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

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()
+

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