Add translation for simulated robot

- Translate RobotCommand messages for the simulated robot
- Add example config file, remove actual config file from being tracked.
- Update gitignore to ignore json files (like the config file), except
  for the example.
- Create proper ROS package for this node.
- Change name of the WHICH ROBOT option in the config file to
  "which_robot".

.gitignore

@@ -1,5 +1,10 @@
+# ignore json config files except for the example
+*.json
+!*.example.json
+
 # vim/gvim swap files
 *.swp
+*.swo
 
 # ignore all OS generated files
 .DS_Store*
@@ -121,5 +126,3 @@ qtcreator-*
 
 # Catkin custom files
 CATKIN_IGNORE
-
-

CMakeLists.txt

@@ -0,0 +1,161 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(sar_robot_translation)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES sar_opal_sender
+#  CATKIN_DEPENDS rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+# add_library(sar_opal_sender
+#   src/${PROJECT_NAME}/sar_opal_sender.cpp
+# )
+
+## Declare a cpp executable
+# add_executable(sar_opal_sender_node src/sar_opal_sender_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(sar_opal_sender_node sar_opal_sender_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(sar_opal_sender_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS sar_opal_sender sar_opal_sender_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_sar_opal_sender.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

README.md

@@ -20,9 +20,9 @@ program expects the configuration file to be called
 "robot\_translation\_config.json". The configuration includes the following
 options:
 
-- which-robot: This ROS node will translate the generic robot commands received
-  into platform-specific robot commands for the robot indicated here, e.g.,
-  JIBO or SPRITE.
+- which\_robot: This ROS node will translate the generic robot commands
+  received into platform-specific robot commands for the robot indicated here.
+  Available robot types include: JIBO, SPRITE, SIMULATED.
 
 If roscore is not running, the program will print a message saying that it is
 unable to register with the master node, and will keep trying to connect.

package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package>
+  <name>sar_robot_translation</name>
+  <version>0.1.0</version>
+  <description>Translates a generic robot command to platform-specific robot commands</description>
+
+  <maintainer email="[email protected]">jakory</maintainer>
+  <license>MIT</license>
+  <url type="website">https://github.com/personal-robots/sar_robot_translation</url>
+  <author email="[email protected]">jakory</author> 
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <run_depend>rospy</run_depend>
+
+</package>

src/robot_translation_config.example.json

@@ -0,0 +1,3 @@
+{
+    "which_robot":"ROBOT_NAME"
+}

src/robot_translation_node.py

@@ -29,6 +29,7 @@
 import json # to read in JSON config file
 from sar_robot_command_msgs.msg import RobotCommand # ROS msgs
 from sar_robot_command_msgs.msg import RobotState # ROS msgs
+from std_msgs.msg import Header # standard ROS Header
 
 # The SAR robot translation node subscribes to the robot_command topic and 
 # translates any robot commands received from the generic format to platform-
@@ -43,13 +44,13 @@ def __init__(self):
         try:
             with open ("robot_translation_config.json") as json_file:
                 json_data = json.load(json_file)
-            print(json_data)
-            self.which_robot = json_data['which-robot']
+            rospy.loginfo("Got config:\n" + str(json_data))
+            self.which_robot = json_data['which_robot']
         except ValueError as e:
-            print('Error! Could not open or parse json config file!'
+            rospy.logerr('Error! Could not open or parse json config file!'
                 + '\n  Did you use valid json?\nError: %s' % e)
         except IOError as e:
-            print('Error! Could not open or could not parse json '
+            rospy.logerr('Error! Could not open or could not parse json '
                    +'config file!\n  Does the file exist in this directory?'
                    + '\nError: %s' % e)
 
@@ -58,32 +59,45 @@ def run_robot_translation_node(self):
         """ wait for robot commands and pass them on """
         # start ros node
         rospy.init_node('robot_translation_node', anonymous=True)
-        rospy.loginfo("Robot translation node starting up!")
+        rospy.loginfo("Robot translation node starting up! Configured to send "
+               "to " + self.which_robot + " robot.")
 
         # subscribe to /robot_command topic to get command messages
         # for the robot
         rospy.Subscriber('robot_command', RobotCommand,
                 self.on_robot_command_msg)
+        rospy.loginfo("Subscribed to 'robot_command' topic.")
 
         # subscribe to /robot_state topic to get status messages from
         # the robot
         rospy.Subscriber('robot_state', RobotState, self.on_robot_state_msg)
+        rospy.loginfo("Subscribed to 'robot_state' topic.")
 
         # publish to robot-specific topic to pass commands to robots
         # if robot is jibo...
-        if (self.which-robot == 'JIBO'):
+        if (self.which_robot == 'JIBO'):
             pass
             # TODO what topic and what message type for Jibo?
             #self.jibo_pub = rospy.Publisher('jibo_command', JiboMessage,
                     #queue_size = 10)
+            #rospy.loginfo("Will publish to 'jibo_command' topic.")
+
         # if robot is a SPRITE robot...
-        elif (self.which-robot == 'SPRITEBOT'):
+        elif (self.which_robot == 'SPRITE'):
             pass
             # TODO what topic and what message type for SPRITE robots?
             #self.sprite_pub = rospy.Publisher('sprite_command', SpriteMessage,
                     #queue_size = 10)
+            #rospy.loginfo("Will publish to 'sprite_command' topic.")
+
+        # if robot is a simulated robot...
+        elif (self.which_robot == 'SIMULATED'):
+            self.robot_sim_pub = rospy.Publisher('robot_sim_command', 
+                    RobotCommand, queue_size = 10)
+            rospy.loginfo("Will publish to 'robot_sim_command' topic.")
+
         # fill in for any other robots
-        #elif (self.which-robot == 'OTHER_ROBOT'):
+        #elif (self.which_robot == 'OTHER_ROBOT'):
             # TODO what topic and what message type for other robot?
             #self.other_pub = rospy.Publisher('other_command', OtherMessage,
                     #queue_size = 10)
@@ -92,34 +106,37 @@ def run_robot_translation_node(self):
         rospy.spin()
 
 
-    def on_robot_state_msg(data):
-        """ receive status messages from robots """
-        print(data)
+    def on_robot_state_msg(self, data):
+        """ Receive status messages from robots """
+        rospy.loginfo("Got message:\n" + str(data))
         # TODO do something with robot status messages?
 
 
-    def on_robot_command_msg(data):
-        """ translate the robot command for the specific platform! """
-        print(data)
+    def on_robot_command_msg(self, data):
+        """ Translate the robot command for the specific platform! """
+        rospy.loginfo("Got message:\n" + str(data))
 
         # TODO check that data is valid after we finalize command format!
 
         # pass command in platform-specific way
-        # send to jibo...
-        if (self.which-robot == 'JIBO'):
+        # send to Jibo...
+        if (self.which_robot == 'JIBO'):
             self.send_to_jibo(data)
-        # send to spritebot...
-        elif (self.which-robot == 'SPRITEBOT'):
-            self.send_to_spritebot(data)
+        # send to SPRITE robot...
+        elif (self.which_robot == 'SPRITE'):
+            self.send_to_sprite(data)
+        # send to simulated robot...
+        elif (self.which_robot == 'SIMULATED'):
+            self.send_to_simulated(data)
         # fill in for any other robots:
-        #elif (self.which-robot == 'OTHER_ROBOT'):
+        #elif (self.which_robot == 'OTHER_ROBOT'):
             #self.send_to_other_robot(data)
 
 
-    def send_to_sprite(data):
-        """ translate robot command to format SPRITE robot uses """
+    def send_to_sprite(self, data):
+        """ Translate robot command to format SPRITE robot uses """
         # TODO send command to SPRITE robot
-        print("TODO send to sprite robot!")
+        rospy.logwarn("TODO send to sprite robot!")
 
         # TODO create ROS message that will be sent to the SPRITE
         # robot
@@ -163,24 +180,38 @@ def send_to_sprite(data):
         #rospy.loginfo(msg)
 
 
-    def send_to_jibo(data):
-        """ translate robot command to format jibo uses """
+    def send_to_jibo(self, data):
+        """ Translate robot command to format Jibo uses """
         # TODO send command to jibo
-        print("TODO send to jibo!")
+        rospy.logwarn("TODO send to jibo!")
 
         # TODO create message to send
         # TODO fill message with data from RobotCommand
         # TODO send message
 
 
-    def send_to_other_robot(data):
-        """ translate robot command to format other robot uses """
-        # fill in to send commands to any other robot
-        print("TODO send to other robot!")
+    def send_to_simulated(self, data):
+        """ Translate robot command to format simulated robot uses """
+        # Simulated robot just gets standard RobotCommand messages
+        msg = RobotCommand()
+        # add header
+        msg.header = Header()
+        msg.header.stamp = rospy.Time.now()
+        # add message content
+        msg.command = data.command
+        msg.properties = data.properties
+        # send message
+        self.robot_sim_pub.publish(msg)
+        rospy.loginfo("Forwarding message to simulated robot:\n" + str(msg))
+
 
+    def send_to_other_robot(self, data):
+        """ Translate robot command to format other robot uses """
+        # fill in to send commands to other robot
+        rospy.logwarn("TODO send to other robot!")
 
 
 if __name__ == '__main__':
     # run the node!
-        node = robot_translation()
-        node.run_robot_translation_node()
+    node = robot_translation()
+    node.run_robot_translation_node()