class Clearpath:
def __init__(self):
rospy.init_node('clearpath_base')
self.port = rospy.get_param('~port', '')
self.cmd_fill = rospy.get_param('~cmd_fill', True)
if self.port != '':
# Serial port
rospy.loginfo("Using port %s", self.port)
self.horizon = Horizon(transport = transports.Serial,
transport_args = { 'port': self.port })
else:
# Not specified. Autodetect.
rospy.loginfo("Using port autodetection")
self.horizon = Horizon(transport = transports.Serial.autodetect)
announce_pub = rospy.Publisher('/clearpath/announce/robots', String, latch=True)
announce_pub.publish(rospy.get_namespace());
self.freq_pub = rospy.Publisher('cmd_freq', Float32, latch=True)
self.cmd_sub = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_handler)
self.cmd_msg = Twist()
self.cmd_time = rospy.Time.now()
self.cmd_event = Event()
def run(self):
previous_error = False
cmd_timeout = False
# Reconnection loop.
while not rospy.is_shutdown():
if previous_error:
rospy.sleep(RECONNECT_TIMEOUT)
try:
if self.horizon.opened:
self.horizon.close()
self.horizon.open()
self.unsub_all()
rospy.loginfo("Connection successful on %s", self.horizon)
previous_error = False
# Fetch and publish robot information.
self.horizon.acks(True);
platform_name = self.horizon.request_platform_name(subscription=0)
platform_info = self.horizon.request_platform_info(subscription=0)
firmware_info = self.horizon.request_firmware_info(subscription=0)
robot_msg = data.pkg_robot_info(platform_name, platform_info, firmware_info)
robot_pub = rospy.Publisher('robot', ClearpathRobot, latch=True)
robot_pub.publish(robot_msg);
self.do_subscriptions()
self.horizon.acks(False);
while not rospy.is_shutdown():
self.cmd_event.wait(CMD_TIME_MAX)
if (rospy.Time.now() - self.cmd_time).to_sec() > CMD_TIME_MAX:
# Timed out waiting on command message. Send zeros instead to
# keep connection alive.
self.cmd_msg = Twist()
if not cmd_timeout:
rospy.loginfo("User commands timed out.")
cmd_timeout = True
else:
if cmd_timeout:
rospy.loginfo("Receiving user commands.")
cmd_timeout = False
self.cmd_event.clear()
try:
if not cmd_timeout or self.cmd_fill:
self.cmd_vel(self.cmd_msg.linear.x, self.cmd_msg.angular.z)
rospy.sleep(CMD_TIME_MIN)
except:
rospy.logerr("Problem issuing command to platform. Attempting reconnection.")
break
if (rospy.Time.now() - self.data_time).to_sec() > DATA_TIMEOUT:
rospy.logerr("Problem receiving data from platform. Attempting reconnection.")
break
except utils.TransportError:
if not previous_error:
rospy.logerr("Connection error on %s. Will retry every second.", self.port if self.port != '' else '/dev/ttyUSB* or /dev/ttyS*')
previous_error = True
self.horizon.close()
def cmd_vel_handler(self, msg):
""" Received command from ROS. Signal the main thread to send it to robot. """
last_cmd_time = self.cmd_time
self.cmd_msg = msg
self.cmd_time = rospy.Time.now()
self.cmd_event.set()
self.freq_pub.publish((last_cmd_time - self.cmd_time).to_sec())
def cmd_vel(self, linear_velocity, angular_velocity):
rospy.logerror('Base class contains no command model. Use kinematic.py or raw.py.')
raise NotImplementedError()
def unsub_all(self):
for topic, cls in data.msgs.items():
subscribe_func = getattr(self.horizon, 'request_' + topic)
try:
subscribe_func(0xffff)
except:
pass
def do_subscriptions(self):
self.publishers = {}
for topic, frequency in rospy.get_param('~data', {}).items():
if not topic in self.publishers:
self.publishers[topic] = rospy.Publisher('data/' + topic,
data.msgs[topic],
latch=True)
subscribe_func = getattr(self.horizon, 'request_' + topic)
subscribe_func(frequency)
rospy.loginfo("Successfully returning data: request_%s", topic)
self.horizon.add_handler(self._receive)
self.data_time = rospy.Time.now()
def _receive(self, name, payload, timestamp):
self.data_time = rospy.Time.now()
try:
pkg_func = getattr(data, 'pkg_' + name)
except AttributeError:
rospy.loginfo("Unhandled Clearpath message of type: %s", name)
else:
msg = pkg_func(payload)
msg.header.stamp = rospy.Time.now()
if name in self.publishers:
self.publishers[name].publish(msg)
else:
rospy.loginfo("Unsubscribed Clearpath message of type: %s", name)
class Clearpath:
def __init__(self):
rospy.init_node('clearpath_base')
self.port = rospy.get_param('~port', '')
self.cmd_fill = rospy.get_param('~cmd_fill', True)
if self.port != '':
# Serial port
rospy.loginfo("Using port %s", self.port)
self.horizon = Horizon(transport=transports.Serial,
transport_args={'port': self.port})
else:
# Not specified. Autodetect.
rospy.loginfo("Using port autodetection")
self.horizon = Horizon(transport=transports.Serial.autodetect)
announce_pub = rospy.Publisher('/clearpath/announce/robots',
String,
latch=True)
announce_pub.publish(rospy.get_namespace())
self.freq_pub = rospy.Publisher('cmd_freq', Float32, latch=True)
self.cmd_sub = rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_handler)
self.cmd_msg = Twist()
self.cmd_time = rospy.Time.now()
self.cmd_event = Event()
def run(self):
previous_error = False
cmd_timeout = False
# Reconnection loop.
while not rospy.is_shutdown():
if previous_error:
rospy.sleep(RECONNECT_TIMEOUT)
try:
if self.horizon.opened:
self.horizon.close()
self.horizon.open()
self.unsub_all()
rospy.loginfo("Connection successful on %s", self.horizon)
previous_error = False
# Fetch and publish robot information.
self.horizon.acks(True)
platform_name = self.horizon.request_platform_name(
subscription=0)
platform_info = self.horizon.request_platform_info(
subscription=0)
firmware_info = self.horizon.request_firmware_info(
subscription=0)
robot_msg = data.pkg_robot_info(platform_name, platform_info,
firmware_info)
robot_pub = rospy.Publisher('robot',
ClearpathRobot,
latch=True)
robot_pub.publish(robot_msg)
self.do_subscriptions()
self.horizon.acks(False)
while not rospy.is_shutdown():
self.cmd_event.wait(CMD_TIME_MAX)
if (rospy.Time.now() -
self.cmd_time).to_sec() > CMD_TIME_MAX:
# Timed out waiting on command message. Send zeros instead to
# keep connection alive.
self.cmd_msg = Twist()
if not cmd_timeout:
rospy.loginfo("User commands timed out.")
cmd_timeout = True
else:
if cmd_timeout:
rospy.loginfo("Receiving user commands.")
cmd_timeout = False
self.cmd_event.clear()
try:
if not cmd_timeout or self.cmd_fill:
self.cmd_vel(self.cmd_msg.linear.x,
self.cmd_msg.angular.z)
rospy.sleep(CMD_TIME_MIN)
except:
rospy.logerr(
"Problem issuing command to platform. Attempting reconnection."
)
break
if (rospy.Time.now() -
self.data_time).to_sec() > DATA_TIMEOUT:
rospy.logerr(
"Problem receiving data from platform. Attempting reconnection."
)
break
except utils.TransportError:
if not previous_error:
rospy.logerr(
"Connection error on %s. Will retry every second.",
self.port
if self.port != '' else '/dev/ttyUSB* or /dev/ttyS*')
previous_error = True
self.horizon.close()
def cmd_vel_handler(self, msg):
""" Received command from ROS. Signal the main thread to send it to robot. """
last_cmd_time = self.cmd_time
self.cmd_msg = msg
self.cmd_time = rospy.Time.now()
self.cmd_event.set()
self.freq_pub.publish((last_cmd_time - self.cmd_time).to_sec())
def cmd_vel(self, linear_velocity, angular_velocity):
rospy.logerror(
'Base class contains no command model. Use kinematic.py or raw.py.'
)
raise NotImplementedError()
def unsub_all(self):
for topic, cls in data.msgs.items():
subscribe_func = getattr(self.horizon, 'request_' + topic)
try:
subscribe_func(0xffff)
except:
pass
def do_subscriptions(self):
self.publishers = {}
for topic, frequency in rospy.get_param('~data', {}).items():
if not topic in self.publishers:
self.publishers[topic] = rospy.Publisher('data/' + topic,
data.msgs[topic],
latch=True)
subscribe_func = getattr(self.horizon, 'request_' + topic)
subscribe_func(frequency)
rospy.loginfo("Successfully returning data: request_%s", topic)
self.horizon.add_handler(self._receive)
self.data_time = rospy.Time.now()
def _receive(self, name, payload, timestamp):
self.data_time = rospy.Time.now()
try:
pkg_func = getattr(data, 'pkg_' + name)
except AttributeError:
rospy.loginfo("Unhandled Clearpath message of type: %s", name)
else:
msg = pkg_func(payload)
msg.header.stamp = rospy.Time.now()
if name in self.publishers:
self.publishers[name].publish(msg)
else:
rospy.loginfo("Unsubscribed Clearpath message of type: %s",
name)
- taken from original example of Clearpath Robotics
"""
from clearpath.horizon import Horizon
from clearpath.horizon.transports import Serial
import time
horizon = Horizon(transport=Serial, transport_args={'port':'/dev/ttyUSB1'})
#horizon = Horizon()
horizon.open()
def status_handler(code, payload, timestamp):
print(payload.print_format())
#horizon.add_handler(status_handler, request = 'system_status')
#horizon.request_system_status(subscription = 1)
for i in range(1, 3):
left_percent = right_percent = i * 10
horizon.set_differential_output(left_percent, right_percent)
time.sleep(0.5)
while True:
horizon.set_differential_output(30, 33)
time.sleep(0.5)
time.sleep(1)
horizon.close()