velodyne_configuration: Add example node. Add more information to the…

… response of the special commands.

velodyne_configuration/CMakeLists.txt

@@ -25,5 +25,6 @@ include_directories(
 
 catkin_install_python(PROGRAMS
     nodes/configuration_node.py
+    nodes/example_node.py
         DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

velodyne_configuration/nodes/configuration_node.py

@@ -9,8 +9,8 @@
 from velodyne_msgs.srv import VelodyneRequestConfigurationResponse
 from velodyne_msgs.srv import VelodyneSetConfiguration
 from velodyne_msgs.srv import VelodyneSetConfigurationResponse
-from velodyne_msgs.srv import VelodyneSpecialCommand
-from velodyne_msgs.srv import VelodyneSpecialCommandResponse
+from velodyne_msgs.srv import VelodyneSpecialCommands
+from velodyne_msgs.srv import VelodyneSpecialCommandsResponse
 
 import os
 
@@ -30,20 +30,15 @@ class ConfiguratorNode:
         VelodyneReturnMode.DUAL_CONF: 'Dual_conf',
     }
 
-    # TODO: Diagnostics with check
-    # TODO: Reset Sensor
-    # TODO: snapshot file download
-    # TODO: save current config
-
     def __init__(self):
         rospy.loginfo("Velodyne configuration node initializing")
         rospy.init_node("velodyne_configuration_node", anonymous=False)
 
         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', VelodyneSetConfiguration,
+        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', VelodyneSpecialCommand,
+        self._cmd_service = rospy.Service('/sensor/lidar/vls128_roof/special_command', VelodyneSpecialCommands,
                                           self.send_command)
 
         velodyne_ip = '192.168.3.201'
@@ -66,21 +61,31 @@ def __init__(self):
         self.configurator = Configurator(velodyne_ip)  # Loads config object with current state
 
     def send_command(self, request):
-        response = VelodyneSpecialCommandResponse()
+        response = VelodyneSpecialCommandsResponse()
         try:
-            if request.command == VelodyneSpecialCommand.DIAGNOSTICS:
+            if request.command == VelodyneSpecialCommands.DIAGNOSTICS:
                 diag = self.configurator.get_diagnostics()
+                response.all_in_range = True
+                outside_range = []
                 for b in diag:
                     for n in diag[b]:
                         in_range = self.configurator.check_diagnostics_parameter(b, n, diag[b][n])
                         if not in_range:
                             print("parameter %s %s is outside operational ranges [%f]", b, n, diag[b][n])
-            elif request.command == VelodyneSpecialCommand.RESET_SENSOR:
+                            response.all_in_range = False
+                            outside_range.append(b+"_"+n)
+                response.parameters = outside_range
+                response.success = True
+
+            elif request.command == VelodyneSpecialCommands.RESET_SENSOR:
                 self.configurator.reset_sensor()
-            elif request.command == VelodyneSpecialCommand.GET_SNAPSHOT:
+                response.success = True
+            elif request.command == VelodyneSpecialCommands.DOWNLOAD_SNAPSHOT:
                 self.configurator.download_snapshot(self.snapshot_path)
-            elif request.command == VelodyneSpecialCommand.SAVE_CONFIG:
+                response.success = True
+            elif request.command == VelodyneSpecialCommands.SAVE_CONFIG:
                 self.configurator.save_current_config_to_sensor()
+                response.success = True
             else:
                 response.success = False
         except RuntimeError as e:

velodyne_configuration/nodes/example_node.py

@@ -6,56 +6,136 @@
 from velodyne_curl_communication_lib.velodyne_curl_config import Configurator
 from velodyne_msgs.msg import VelodyneReturnMode
 import os
+from velodyne_msgs.srv import VelodyneRequestConfiguration
+from velodyne_msgs.srv import VelodyneRequestConfigurationResponse
+from velodyne_msgs.srv import VelodyneSetConfiguration
+from velodyne_msgs.srv import VelodyneSetConfigurationResponse
+from velodyne_msgs.srv import VelodyneSpecialCommands
+from velodyne_msgs.srv import VelodyneSpecialCommandsResponse
 
 
 class ConfiguratorNode:
     def __init__(self):
-        rospy.init_node("velodyne_configuration_node", anonymous=True)
-        rospy.loginfo("Velodyne configuration node initializing")
+        rospy.init_node("velodyne_configuration_test_node", anonymous=True)
+        rospy.loginfo("Velodyne configuration test node initializing")
 
-        velodyne_ip = '192.168.3.201'
-        if rospy.has_param('velodyne_ip'):
-            velodyne_ip = rospy.get_param('~velodyne_ip')
+        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._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.stamp = rospy.Time.now()
+        current_config = self._get_config_srv_proxy (get_config_request)
+
+        if current_config.success:
+            print("received current configuration")
+            print("gps_pps_state")
+            print(current_config.gps_pps_state)
+            print("gps_position")
+            print(current_config.gps_position)
+            print("tod_time")
+            print(current_config.tod_time)
+            print("usectoh")
+            print(current_config.usectoh)
+            print("motor_state")
+            print(current_config.motor_state)
+            print("ns_per_rev")
+            print(current_config.ns_per_rev)
+            print("rpm")
+            print(current_config.rpm)
+            print("fov_start")
+            print(current_config.fov_start)
+            print("fov_end")
+            print(current_config.fov_end)
+            print("returns")
+            print(current_config.returns.return_mode)
+            print("phase")
+            print(current_config.phase)
+            print("phaselock")
+            print(current_config.phaselock)
+            print("laser")
+            print(current_config.laser)
+            print("host_addr")
+            print(current_config.host_addr)
+            print("host_dport")
+            print(current_config.host_dport)
+            print("host_tport")
+            print(current_config.host_tport)
+            print("net_addr")
+            print(current_config.net_addr)
+            print("net_mask")
+            print(current_config.net_mask)
+            print("net_gateway")
+            print(current_config.net_gateway)
         else:
-            rospy.loginfo("velodyne_ip parameter not found, using default ip %s", velodyne_ip)
-
-        rospy.loginfo("Using sensor IP: %s", velodyne_ip)
-
-        self.configurator = Configurator(velodyne_ip)
-        print("Changing return mode")
-        self.configurator.set_setting("returns", VelodyneReturnMode.DUAL)
-        time.sleep(10)
-        print("get diagnostics")
-        d = self.configurator.get_diagnostics()
-        print("diagnostics:")
-        print(d)
-        time.sleep(10)
-        state = self.configurator.get_current_configuration()
-        print("state")
-        print(state)
-        time.sleep(10)
-        print("Turning the lasers off")
-        self.configurator.set_setting("laser", "off")
-        time.sleep(10)
-        print("Turning the laser on")
-        self.configurator.set_setting("laser", "on")
-        time.sleep(10)
-        print("Setting return mode Strong")
-        self.configurator.set_setting("returns", VelodyneReturnMode.STRONGEST)
-        time.sleep(10)
-        print("Getting diagnostics")
-        diagnostics = self.configurator.get_diagnostics()
-        print("diagnostics")
-        print(diagnostics)
-        print("Resetting sensor")
-        self.configurator.reset_sensor()
-        time.sleep(15)
-        print("Downloading snapshot")
-        home = os.environ.get('HOME')
-        self.configurator.download_snapshot(home+"/Downloads")
-
-        rospy.sleep(rospy.Duration.from_sec(10))
-        print("done")
+            print("fail to get configuration")
+            raise RuntimeError
+
+        set_config_request = self.request_config_from_current_config(current_config)
+        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_response = self._special_config_srv_proxy(spc_cmd_config_request)
+
+        if spc_cmd_response.succes:
+            print("all parameters within the operational range")
+        else:
+            print("parameter outside its operational range!")
+            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_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_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.stamp = rospy.Time.now()
+        current_config = self._get_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
+        set_config_request.fov_start = 90
+        set_config_request.fov_end = 180
+        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.stamp = rospy.Time.now()
+
+        set_config_request.rpm = current.rpm
+        set_config_request.fov_start = current.fov_start
+        set_config_request.fov_end = current.fov_end
+        set_config_request.returns = current.returns
+        set_config_request.phase = current.phase
+        set_config_request.phaselock = current.phaselock
+        set_config_request.laser = current.laser
+        set_config_request.host_addr = current.host_addr
+        set_config_request.host_dport = current.host_dport
+        set_config_request.host_tport = current.host_tport
+        set_config_request.net_addr = current.net_addr
+        set_config_request.net_mask = current.net_mask
+        set_config_request.net_gateway = current.net_gateway
+
+        return set_config_request
+
 
 if __name__ == "__main__":
     try:

velodyne_configuration/src/velodyne_curl_communication_lib/velodyne_curl_config.py

@@ -194,7 +194,10 @@ def __init__(self, velodyne_ip):
 
     def test_connection(self):
         # Test connection to sensor
-        status = self.get_current_configuration()
+        try:
+            status = self.get_current_configuration()
+        except RuntimeError as e:
+            raise RuntimeError from e
         print('Sensor laser is %s, motor rpm is %s',
               status['laser']['state'], status['motor']['rpm'])
 
@@ -407,6 +410,7 @@ def get_current_configuration(self):
         try:
             status = self._request_json('get_status')
         except Exception as e:
+            print(e)
             raise RuntimeError from e
 
         # Only this information is in the status response

velodyne_msgs/CMakeLists.txt

@@ -16,6 +16,7 @@ add_service_files (
         FILES
         VelodyneSetConfiguration.srv
         VelodyneRequestConfiguration.srv
+        VelodyneSpecialCommands.srv
 
 )
 

velodyne_msgs/srv/VelodyneSpecialCommand.srv → velodyne_msgs/srv/VelodyneSpecialCommands.srv

@@ -1,12 +1,14 @@
 # request
 uint8 DIAGNOSTICS                    = 0
 uint8 RESET_SENSOR                   = 1
-uint8 GET_SNAPSHOT                   = 2
+uint8 DOWNLOAD_SNAPSHOT              = 2
 uint8 SAVE_CONFIG                    = 3
 
 uint8 command
 
 ---
 #response
 time stamp
-bool success
+bool success
+bool all_in_range
+string[] parameters