Merge pull request ros-industrial#93 from gavanderhoorn/rob_sim_get_r…

…obot_info

Add GetRobotInfo service server implementation to Robot Simulator

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>