diff --git a/industrial_robot_simulator/industrial_robot_simulator b/industrial_robot_simulator/industrial_robot_simulator
index 15684129..39c1caad 100755
--- a/industrial_robot_simulator/industrial_robot_simulator
+++ b/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:
diff --git a/industrial_robot_simulator/package.xml b/industrial_robot_simulator/package.xml
index d8354004..f8f65e36 100644
--- a/industrial_robot_simulator/package.xml
+++ b/industrial_robot_simulator/package.xml
@@ -16,6 +16,7 @@
trajectory_msgs
industrial_robot_client
industrial_msgs
+ python-rospkg
roscpp
std_msgs
sensor_msgs
@@ -23,4 +24,5 @@
trajectory_msgs
industrial_robot_client
industrial_msgs
+ python-rospkg