robot_simulator: add GetRobotInfo svc server.

Users can override the information returned by the get_robot_info service
server. This can be useful if multiple instances of the simulator are
used in a ROS application, and each needs to be uniquely identified.

Additionally, this makes it possible to return information taken from real
controllers and manipulators, in order to provide a more 'real' simulation.

Users can enable the override by setting the private parameter 'robot_info' in
a launch file like so:

  <node .. type="industrial_robot_simulator" name=".." ..>
    <rosparam param="robot_info">
      controller:
        model: 'My Robot Controller'
        serial_number: 'ABC-123'
        sw_version: 'V1.23/ABC'
      robots:
        - model: 'My Robot Model'
          serial_number: '123456'
          sw_version: 'V4.56/DEF'
    </rosparam>
  </node>

As the Industrial Robot Simulator currently simulates only a single robot /
motion group, specifying multiple robot entries will result in only the first
of the list to be used.

Fields absent from the rosparam parameter will result in empty strings being
assigned to the corresponding fields in the 'controller' and 'robot' parts of
the service response.

industrial_robot_simulator/industrial_robot_simulator

@@ -14,8 +14,11 @@ from industrial_msgs.msg import RobotStatus
 from trajectory_msgs.msg import JointTrajectory
 from trajectory_msgs.msg import JointTrajectoryPoint
 
+# Services
+from industrial_msgs.srv import GetRobotInfo, GetRobotInfoResponse
+
 # Reference
-from industrial_msgs.msg import TriState, RobotMode
+from industrial_msgs.msg import TriState, RobotMode, ServiceReturnCode, DeviceInfo
 
 
 
@@ -233,10 +236,22 @@ class IndustrialRobotSimulatorNode():
         rospy.logdebug('Setting up publish worker with period (sec): %s', str(period.to_sec()))
         rospy.Timer(period, self.publish_worker)
 
+        # GetRobotInfo service server and pre-cooked svc response
+        self.get_robot_info_response = self._init_robot_info_response()
+        self.svc_get_robot_info = rospy.Service('get_robot_info', GetRobotInfo, self.cb_svc_get_robot_info)
+
         # Clean up init
         rospy.on_shutdown(self.motion_ctrl.shutdown)
 
 
+    """
+    Service callback for GetRobotInfo() service. Returns fake information.
+    """
+    def cb_svc_get_robot_info(self, req):
+        # return cached response instance
+        return self.get_robot_info_response
+
+
     """
     The publish worker is executed at a fixed rate.  This publishes the various
     state and status information for the robot.
@@ -366,6 +381,48 @@ class IndustrialRobotSimulatorNode():
 
         return ordered_values
 
+    """
+    Constructs a GetRobotInfoResponse instance with either default data,
+    or data provided by the user.
+    """
+    def _init_robot_info_response(self):
+        if not rospy.has_param('~robot_info'):
+            # if user did not provide data, we generate some
+            import rospkg
+            rp = rospkg.RosPack()
+            irs_version = rp.get_manifest('industrial_robot_simulator').version
+            robot_info = dict(
+                controller=dict(
+                    model='Industrial Robot Simulator Controller',
+                    serial_number='0123456789',
+                    sw_version=irs_version),
+                robots=[
+                    dict(
+                        model='Industrial Robot Simulator Manipulator',
+                        serial_number='9876543210',
+                        sw_version=irs_version)
+                ])
+        else:
+            # otherwise use only the data user has provided (and nothing more)
+            robot_info = rospy.get_param('~robot_info')
+
+        resp = GetRobotInfoResponse()
+        resp.controller = DeviceInfo(**robot_info['controller'])
+
+        # add info on controlled robot / motion group
+        if len(robot_info['robots']) > 0:
+            robot = robot_info['robots'][0]
+            resp.robots.append(DeviceInfo(**robot))
+
+        if len(robot_info['robots']) > 1:
+            # simulator simulates a single robot / motion group
+            rospy.logwarn("Multiple robots / motion groups defined in "
+                "'robot_info' parameter, ignoring all but first element")
+
+        # always successfull
+        resp.code.val = ServiceReturnCode.SUCCESS
+        return resp
+
 
 if __name__ == '__main__':
     try:

industrial_robot_simulator/package.xml

@@ -16,11 +16,13 @@
   <build_depend>trajectory_msgs</build_depend>
   <build_depend>industrial_robot_client</build_depend>
   <build_depend>industrial_msgs</build_depend>
+  <build_depend>python-rospkg</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>control_msgs</run_depend>
   <run_depend>trajectory_msgs</run_depend>
   <run_depend>industrial_robot_client</run_depend>
   <run_depend>industrial_msgs</run_depend>
+  <run_depend>python-rospkg</run_depend>
 </package>