From 9af842e8be278dd73d2dc3cc50ee679bc8e46f8b Mon Sep 17 00:00:00 2001 From: jakory Date: Fri, 17 Jun 2016 11:25:43 -0400 Subject: [PATCH] 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 | 7 +- CMakeLists.txt | 161 ++++++++++++++++++++++ README.md | 6 +- package.xml | 16 +++ src/robot_translation_config.example.json | 3 + src/robot_translation_config.json | 3 - src/robot_translation_node.py | 95 ++++++++----- 7 files changed, 251 insertions(+), 40 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 package.xml create mode 100644 src/robot_translation_config.example.json delete mode 100644 src/robot_translation_config.json diff --git a/.gitignore b/.gitignore index 201b072..df4ecfe 100644 --- a/.gitignore +++ b/.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 - - diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..744af0b --- /dev/null +++ b/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) diff --git a/README.md b/README.md index 821a895..5eaddc9 100644 --- a/README.md +++ b/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. diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..f5ea36b --- /dev/null +++ b/package.xml @@ -0,0 +1,16 @@ + + + sar_robot_translation + 0.1.0 + Translates a generic robot command to platform-specific robot commands + + jakory + MIT + https://github.com/personal-robots/sar_robot_translation + jakory + + catkin + rospy + rospy + + diff --git a/src/robot_translation_config.example.json b/src/robot_translation_config.example.json new file mode 100644 index 0000000..ffd8ed1 --- /dev/null +++ b/src/robot_translation_config.example.json @@ -0,0 +1,3 @@ +{ + "which_robot":"ROBOT_NAME" +} diff --git a/src/robot_translation_config.json b/src/robot_translation_config.json deleted file mode 100644 index 4ebc7e4..0000000 --- a/src/robot_translation_config.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "which-robot":"ROBOT_NAME" -} diff --git a/src/robot_translation_node.py b/src/robot_translation_node.py index 02539ea..2543087 100755 --- a/src/robot_translation_node.py +++ b/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()