Commit 812e3c34 authored by Tommy Olofsson's avatar Tommy Olofsson
Browse files

Prepared the ROS part of the ORCA bridge.

parent 6f46410a
......@@ -3,3 +3,11 @@ Makefile
build/
msg_gen/
src/ros_extctrl
CMakeCache.txt
CMakeFiles/
catkin/
catkin_generated/
cmake_install.cmake
devel/
gtest/
srv_gen/
......@@ -17,9 +17,9 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
rosbuild_genmsg()
# rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
rosbuild_gensrv()
#common commands for building c++ executables and libraries
#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
......
<bridge name="extctrl_bridge" port="7357">
<!-- TODO: What controller do we have time to integrate with? -->
<!-- We will probably add a custom conversion here... -->
<topics_in>
<topic name="/extctrl_status" />
</topics_in>
<topics_out>
<topic name="/extctrl_cmd" />
</topics_out>
</bridge>
#!/usr/bin/env python
# The bridge generator cannot make an external service look lika a
# ROS service, only the opposite. That would seem a better fit here
# though...
import roslib; roslib.load_manifest('ros_extctrl')
import rospy
from ros_extctrl.msg import ExtCtrlCmd, ExtCtrlStatus
from ros_extctrl.srv import ExtCtrlStart
from geometry_msgs.msg import PoseStamped
def handle_extctrl_status(data):
print("Controller responded with %d" % data.status)
def interact(pub):
def interact():
while not rospy.is_shutdown():
rospy.wait_for_service('ExtCtrlStart')
start_controller = rospy.ServiceProxy('ExtCtrlStart', ExtCtrlStart)
line = ''
try:
line = raw_input("Choose a controller to activate:")
except EOFError:
print('\nBye')
break
ctrl = 0
aua_id = 0
try:
ctrl = int(line)
aua_id = int(line)
except ValueError:
print("%s is not a number" % line)
continue
msg = ExtCtrlCmd()
msg.aua_id = ctrl
msg.pose.pose.position.x = 0.0
msg.pose.pose.position.y = 0.0
msg.pose.pose.position.z = 0.0
msg.pose.pose.orientation.x = 0.0
msg.pose.pose.orientation.y = 0.0
msg.pose.pose.orientation.z = 0.0
msg.pose.pose.orientation.w = 0.0
pub.publish(msg)
pose = PoseStamped()
pose.pose.position.x = 0.0
pose.pose.position.y = 0.0
pose.pose.position.z = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
resp = None
try:
resp = start_controller(aua_id, pose)
except rospy.ServiceException as exc:
print("Service did not process request: " + str(exc))
continue
print("%s" % resp)
if __name__ == '__main__':
rospy.init_node('extctrl_example')
pub = rospy.Publisher('extctrl_cmd', ExtCtrlCmd)
sub = rospy.Subscriber('extctrl_status', ExtCtrlStatus,
handle_extctrl_status)
interact(pub)
rospy.init_node('extctrl_usage_example')
interact()
#!/usr/bin/env python
import roslib; roslib.load_manifest('ros_extctrl')
import rospy
from optparse import OptionParser
import sys
from ros_extctrl.srv import ExtCtrlStart
class OrcaBridge(object):
def __init__(self, address, port):
# ORCA setup
# TODO: Talk to Mahdi about the simulik signals to use.
#self._orca_conn = orca.connection(address, port)
#self._orca_enc_start = self.orca_conn.open_input(['some_sample'])
# ROS setup
# Wrapper to use member funcion as service callback.
def cb(req, insn=self):
return insn._orca_start(req)
self.start_srv = rospy.Service('ExtCtrlStart',
ExtCtrlStart, cb)
def _orca_start(self, req):
# TODO: Init. labcomm type and send to simulink.
# self._orca_enc_start.encode(lc_data, lc_type)
print(req)
return req.aua_id
def run(self):
rospy.spin()
if __name__ == '__main__':
op = OptionParser()
op.add_option("-a", "--address", dest="address",
help="IP address of ORCA server")
op.add_option("-p", "--port", dest="port",
help="TCP port of ORCA server")
(opts, args) = op.parse_args()
if opts.address is None or opts.port is None:
sys.stderr.write("Use '--help' for usage.\n")
sys.exit(1)
rospy.init_node('orca_bridge')
b = OrcaBridge(opts.address, opts.port)
b.run()
uint32 aua_id # Controller ID (0 to deactivate)
geometry_msgs/PoseStamped pose
---
uint32 status
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment