class RiCBattery(Device):
def __init__(self, param, output):
Device.__init__(self, param.getBatteryName(), output)
self._pub = Publisher('%s' % self._name, Float32, queue_size=param.getBatteryPubHz())
self._haveRightToPublish = False
#KeepAliveHandler(self._name, Float32)
def publish(self, data):
msg = Float32()
msg.data = data
self._pub.publish(msg)
def getType(self): return Battery
def checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
if not self._haveRightToPublish and subCheck == '':
self._output.write(PublishRequest(Battery, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and subCheck == 'None':
self._output.write(PublishRequest(Battery, 0, False).dataTosend())
self._haveRightToPublish = False
except: pass
def main_auto():
# initialize ROS node
init_node('auto_mode', anonymous=True)
nh = Publisher('ecu', ECU, queue_size = 10)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
t_i = 0
# get experiment parameters
t_0 = get_param("controller/t_0") # time to start test
t_f = get_param("controller/t_f") # time to end test
FxR_target = get_param("controller/FxR_target")
d_f_target = pi/180*get_param("controller/d_f_target")
# main loop
while not is_shutdown():
# get command signal
(FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target)
# send command signal
ecu_cmd = ECU(FxR, d_f)
nh.publish(ecu_cmd)
# wait
t_i += dt
rate.sleep()
class Program:
def main(self):
if len(sys.argv) >= 3:
rospy.init_node("RiC_PPM")
topicNamePmm = sys.argv[1]
topicNameDiff = sys.argv[2]
# print 'PPM Topic: ' + topicNamePmm + '\n' + 'Diff Topic: ' + topicNameDiff
self.pub = Publisher(topicNameDiff, Twist,queue_size=1)
Subscriber(topicNamePmm, PPM, self.callBack, queue_size=1)
rospy.spin()
def callBack(self, msg):
leftAndRight = 0
upAndDown = 0
if 1450 < msg.channels[0] < 1550: leftAndRight = 0
else: leftAndRight = 0.032 * (float(msg.channels[0])) - 48
if 1450 < msg.channels[1] < 1550: upAndDown = 0
else: upAndDown = 0.032 * (float(msg.channels[1])) - 48
msgPub = Twist()
msgPub.angular.z = leftAndRight
msgPub.linear.x = -upAndDown
self.pub.publish(msgPub)
class RiCURF(Device):
def __init__(self, devId, param):
Device.__init__(self, param.getURFName(devId), None)
self._urfType = param.getURFType(devId)
self._frameId = param.getURFFrameId(devId)
self._pub = Publisher('%s' % self._name, Range, queue_size=param.getURFPubHz(devId))
def getType(self): return self._urfType
def publish(self, data):
msg = Range()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = self._frameId
if self._urfType == URF_HRLV:
msg.min_range = MIN_RANGE_URF_HRLV_MaxSonar
msg.max_range = MAX_RANGE_URF_HRLV_MaxSonar
msg.field_of_view = FIELD_OF_VIEW_URF_HRLV_MaxSonar
else:
msg.min_range = MIN_RANGE_URF_LV_MaxSonar
msg.max_range = MAX_RANGE_URF_LV_MaxSonar
msg.field_of_view = FIELD_OF_VIEW_URF_LV_MaxSonar
msg.radiation_type = Range.ULTRASOUND
msg.range = data
self._pub.publish(msg)
class RiCPPM(Device):
def __init__(self, param, output):
Device.__init__(self, param.getPPMName(), output)
self._pub = Publisher('%s' % self._name, PPM, queue_size=param.getPPMPubHz())
self._haveRightToPublish = False
def getType(self): return PPM
def publish(self, data):
msg = PPM()
msg.channels = data.getChannels()
self._pub.publish(msg)
def checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
if not self._haveRightToPublish and subCheck == '':
self._output.write(PublishRequest(7, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and subCheck == 'None':
self._output.write(PublishRequest(7, 0, False).dataTosend())
self._haveRightToPublish = False
except: pass
class RiCSwitch(Device):
def __init__(self, devId,param, output):
Device.__init__(self, param.getSwitchName(devId), output)
self._pub = Publisher('%s' % self._name, Bool, queue_size=param.getSwitchPubHz(devId))
self._switchId = devId
self._haveRightToPublish = False
def publish(self, data):
msg = Bool()
msg.data = data
self._pub.publish(msg)
def checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
if not self._haveRightToPublish and subCheck == '':
self._output.write(PublishRequest(Button, self._switchId, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and subCheck == 'None':
self._output.write(PublishRequest(Button, self._switchId, False).dataTosend())
self._haveRightToPublish = False
except: pass
def getType(self): return Button
def __init__(self, param, output):
Device.__init__(self, param.getIMUName(), output)
self._frameId = param.getIMUFrameId()
self._angle = param.getIMUOrientation()
self._pub = Publisher("%s_AGQ" % self._name, Imu, queue_size=param.getIMUPubHz())
self._pubMag = Publisher("%s_M" % self._name, MagneticField, queue_size=param.getIMUPubHz())
Service("%s_Calibration" % self._name, calibIMU, self.serviceCallBack)
self._haveRightToPublish = False
def random_temperatures(delay=1):
print('Starting random temperatures...')
pub = Publisher('/cpu/temperature', Temperature)
msg = Temperature()
msg.name = ['cpu0', 'cpu1', 'cpu2', 'cpu2']
while not rospy.is_shutdown():
msg.temperature = [random.randint(30, 80) for i in range(4)]
sleep(delay)
pub.publish(msg)
def random_co2(delay=1):
print('Starting random battery data...')
pub = Publisher('/sensors/co2', Co2Msg)
msg = Co2Msg()
while not rospy.is_shutdown():
msg.header = rospy.Header()
msg.co2_percentage = random.random() * 10
sleep(delay)
pub.publish(msg)
class RiCBattery(Device):
def __init__(self, param):
Device.__init__(self, param.getBatteryName(), None)
self._pub = Publisher('%s' % self._name, Float32, queue_size=param.getBatteryPubHz())
def publish(self, data):
msg = Float32()
msg.data = data
self._pub.publish(msg)
def random_thermal(delay=1):
print('Starting random thermal data...')
pub = Publisher('/sensors/thermal', ThermalMeanMsg)
msg = ThermalMeanMsg()
while not rospy.is_shutdown():
msg.header = rospy.Header()
msg.thermal_mean = random.randint(20, 40)
sleep(delay)
pub.publish(msg)
class RiCDiffCloseLoop(Device):
def __init__(self, param, output):
Device.__init__(self, param.getCloseDiffName(), output)
self._baseLink = param.getCloseDiffBaseLink()
self._odom = param.getCloseDiffOdom()
self._maxAng = param.getCloseDiffMaxAng()
self._maxLin = param.getCloseDiffMaxLin()
self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz())
self._broadCase = TransformBroadcaster()
Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1)
Service('%s/setOdometry' % self._name, set_odom, self.setOdom)
def diffServiceCallback(self, msg):
if msg.angular.z > self._maxAng:
msg.angular.z = self._maxAng
elif msg.angular.z < -self._maxAng:
msg.angular.z = -self._maxAng
if msg.linear.x > self._maxLin:
msg.linear.x = self._maxLin
elif msg.linear.x < -self._maxLin:
msg.linear.x = -self._maxLin
# print msg.angular.z, msg.linear.x
self._output.write(CloseDiffRequest(msg.angular.z, msg.linear.x).dataTosend())
def setOdom(self, req):
self._output.write(CloseDiffSetOdomRequest(req.x, req.y, req.theta).dataTosend())
return set_odomResponse(True)
def publish(self, data):
q = Quaternion()
q.x = 0
q.y = 0
q.z = data[6]
q.w = data[7]
odomMsg = Odometry()
odomMsg.header.frame_id = self._odom
odomMsg.header.stamp = rospy.get_rostime()
odomMsg.child_frame_id = self._baseLink
odomMsg.pose.pose.position.x = data[0]
odomMsg.pose.pose.position.y = data[1]
odomMsg.pose.pose.position.z = 0
odomMsg.pose.pose.orientation = q
self._pub.publish(odomMsg)
traMsg = TransformStamped()
traMsg.header.frame_id = self._odom
traMsg.header.stamp = rospy.get_rostime()
traMsg.child_frame_id = self._baseLink
traMsg.transform.translation.x = data[0]
traMsg.transform.translation.y = data[1]
traMsg.transform.translation.z = 0
traMsg.transform.rotation = q
self._broadCase.sendTransformMessage(traMsg)
class RiCPPM(Device):
def __init__(self, param):
Device.__init__(self, param.getPPMName(), None)
self._pub = Publisher('%s' % self._name, PPM, queue_size=param.getPPMPubHz())
def publish(self, data):
msg = PPM()
msg.channels = data.getChannels()
self._pub.publish(msg)
def random_imu(delay=1):
print('Starting random imu...')
pub = Publisher('/sensors/imu_rpy', ImuRPY)
msg = ImuRPY()
while not rospy.is_shutdown():
msg.roll = random.random() * 50
msg.pitch = random.random() * 50
msg.yaw = random.random() * 50
pub.publish(msg)
sleep(delay)
class RiCSwitch(Device):
def __init__(self, devId,param):
Device.__init__(self, param.getSwitchName(devId), None)
self._pub = Publisher('%s' % self._name, Bool, queue_size=param.getSwitchPubHz(devId))
def publish(self, data):
msg = Bool()
msg.data = data
self._pub.publish(msg)
def random_battery(delay=1):
print('Starting random battery data...')
pub = Publisher('/sensors/battery', BatteryMsg)
msg = BatteryMsg()
msg.name = ['PSU', 'Motors']
while not rospy.is_shutdown():
battery1 = random.randint(18, 25)
battery2 = random.randint(18, 25)
msg.voltage = [battery1, battery2]
sleep(delay)
pub.publish(msg)
def quad_controller(route_queue, position_queue, arming_queue,
trgt_queue, waypoints_queue):
flight_done_pub = Publisher('remove', UInt32, queue_size=1)
while not is_shutdown():
# Take a route
route = route_queue.get(True, None)
loginfo('Received route with id #'+str(route.id)+'.')
if not route.valid:
logerr('Route invalid!')
return
target_pos = trgt_queue.get(True, None)
need_waypoint_id = None
# Determine the point where you want to dump the load
need_waypoint_id = find_waypoint_id(route, (target_pos.latitude, target_pos.longitude))
loginfo('need_waypoint_id: ' + str(need_waypoint_id))
# Push a mission into flight controller
push_mission(waypointWrap(route.route))
loginfo('Mission loaded to flight controller.')
# Set manual mode
set_mode('ACRO')
# Enable motors
arming()
# Set autopilot mode
set_mode('AUTO')
loginfo('Flight!')
# Wainting for arming
sleep(5)
drop_all(arming_queue)
# If you do not have a goal to clear cargo,
# we believe that already dropped
droped = (need_waypoint_id == None)
while arming_queue.get().data and not droped:
waypoints = None
# To not accumulate elements in arming_queue
try:
waypoints = waypoints_queue.get_nowait().waypoints
except Empty:
continue
current_waypoint_id = find_cur_waypoint_id(waypoints)
if current_waypoint_id > need_waypoint_id:
droped = True
drop_cargo()
# TODO: More elegant case for mission finish check
while arming_queue.get().data:
pass
loginfo('Mission #'+str(route.id)+' complete!')
flight_done_pub.publish(route.id)
class ScanSensor:
__MIN_ANGLE = 0.0
__MAX_ANGLE = 2.0*pi
__ANGLE_INCREMENT = 2.0*pi/360.0
def __init__(self, pub_name, frame_id, min_range, max_range):
self._publisher = Publisher(pub_name, LaserScan, queue_size=10)
self._frame_id = frame_id
self._min_range = min_range
self._max_range = max_range
self._last_time = Time.now()
try:
self._hal_proxy = BaseHALProxy()
except BaseHALProxyError:
raise ScanSensorError("Unable to create HAL")
def _get_data(self):
"""
This method is overridden in the subclasses and returns a tuple of ranges (distances), intensities, and delta time
"""
return [], [], 0
def _msg(self, ranges, intensities, scan_time):
new_time = Time.now()
delta_time = new_time - self._last_time
self._last_time = new_time
msg = LaserScan()
msg.header.frame_id = self._frame_id
msg.header.stamp = Time.now()
msg.angle_max = self.__MAX_ANGLE
msg.angle_min = self.__MIN_ANGLE
msg.angle_increment = self.__ANGLE_INCREMENT
# Note: time_increment is the time between measurements, i.e. how often we read the scan and publish it (in
# seconds)
msg.time_increment = 0.1 #delta_time.secs
# Note: scan_time is the time between scans of the laser, i.e., the time it takes to read 360 degrees.
msg.scan_time = 0.1 # scan_time
msg.range_min = float(self._min_range)
msg.range_max = float(self._max_range)
msg.ranges = [min(max(range, msg.range_min), msg.range_max) for range in ranges]
msg.intensities = intensities
return msg
def Publish(self):
ranges, intensities, scan_time = self._get_data()
self._publisher.publish(self._msg(ranges, intensities, scan_time))
class Estimator():
"""
Estimator Node: publishes final result once a second.
Accumulate predictions and publishes a final result every second
"""
def __init__(self, **kwargs):
"""Constructor."""
rospy.init_node('ocular_object_id_averager', anonymous=True)
self.node_name = rospy.get_name()
rospy.on_shutdown(self.shutdown)
loginfo("Initializing " + self.node_name + " node...")
with eh(logger=logfatal, log_msg="Couldn't load parameters",
reraise=True):
self.hz = load_params(['rate']).next()
# Publishers and Subscribers
self.pub = Publisher('final_object_id', SystemOutput)
Subscriber("object_id", RecognizedObject, self.callback)
# self.r = rospy.Rate(self.hz)
# Accumulates Hz items in per second. Ex: 30Hz -> ~30items/sec
self.accumulator = Accumulator(self.hz)
def callback(self, data):
"""Callback that publishes updated predictions when new msg is recv."""
self.accumulator.append(data.object_id)
if self.accumulator.isfull():
rospy.logdebug("Accumulator full. Printing all predictions")
rospy.logdebug("{}".format(self.accumulator))
predictions_rgb, predictions_pcloud = zip(*self.accumulator)
y, y_rgb, y_pcloud = estimate(predictions_rgb, predictions_pcloud)
output_msg = SystemOutput(id_2d_plus_3d=y,
id_2d=y_rgb,
id_3d=y_pcloud)
try:
self.pub.publish(output_msg)
except ValueError as ve:
rospy.logwarn(str(ve))
def run(self):
"""Run (wrapper of ``rospy.spin()``."""
rospy.spin()
def shutdown(self):
"""Shudown hook to close the node."""
loginfo('Shutting down ' + rospy.get_name() + ' node.')
class RiCGPS(Device):
def __init__(self, param, output):
Device.__init__(self, param.getGpsName(), output)
self._frameId = param.getGpsFrameId()
self._pub = Publisher('%s' % self._name, NavSatFix, queue_size=param.getGpsPubHz())
self._haveRightToPublish = False
self._old_fix = 0
self._wd = int(round(time.time() * 1000))
KeepAliveHandler(self._name, NavSatFix)
def publish(self, data):
now = int(round(time.time() * 1000))
msg = NavSatFix()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.latitude = data.getLat()
msg.longitude = data.getLng()
msg.altitude = data.getMeters()
cur_fix = data.getFix()
#print cur_fix
if (cur_fix != self._old_fix):
msg.status.status = 0
self._old_fix = cur_fix
self._wd = now
elif (now - self._wd) < GPS_WD_TIMEOUT:
msg.status.status = 0
else:
msg.status.status = -1
msg.position_covariance_type = 1
msg.position_covariance[0] = data.getHDOP() * data.getHDOP()
msg.position_covariance[4] = data.getHDOP() * data.getHDOP()
msg.position_covariance[8] = 4 * data.getHDOP() * data.getHDOP()
msg.status.service = 1
self._pub.publish(msg)
def getType(self): return GPS
def checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
if not self._haveRightToPublish and subCheck == '':
self._output.write(PublishRequest(GPS, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and subCheck == 'None':
self._output.write(PublishRequest(GPS, 0, False).dataTosend())
self._haveRightToPublish = False
except: pass
def __init__(self, param, output):
Device.__init__(self, param.getIMUName(), output)
self._frameId = param.getIMUFrameId()
self._angle = param.getIMUOrientation()
self._pub = Publisher('%s_AGQ' % self._name, Imu, queue_size=param.getIMUPubHz())
self._pubMag = Publisher('%s_M' % self._name, MagneticField, queue_size=param.getIMUPubHz())
self._pubCalib = Publisher('/imu_calib_publisher', imuCalib, queue_size=param.getIMUPubHz())
Service('/imu_Calibration', calibIMU, self.serviceCallBack)
self._haveRightToPublish = False
self._calib = False
self._xMax = 0.0
self._yMax = 0.0
self._zMax = 0.0
self._xMin = 0.0
self._yMin = 0.0
self._zMin = 0.0
def __init__(self, params, servoNum, output):
Device.__init__(self, params.getServoName(servoNum), output)
self._servoNum = servoNum
self._pub = Publisher('%s/Position' % self._name, Float32, queue_size=params.getServoPublishHz(servoNum))
Subscriber('%s/command' % self._name, Float32, self.servoCallBack)
self._haveRightToPublish = False
class RiCCloseLoopMotor(Device):
def __init__(self, motorNum ,param,output):
Device.__init__(self, param.getCloseLoopMotorName(motorNum), output)
self._motorId = motorNum
self._pub = Publisher('%s/feedback' % self._name, Motor, queue_size=param.getCloseLoopMotorPubHz(motorNum))
Subscriber('%s/command' % self._name, Float32, self.MotorCallback)
def publish(self, data):
msg = Motor()
msg.position = data[0]
msg.velocity = data[1]
self._pub.publish(msg)
def MotorCallback(self, msg):
req = CloseMotorRequest(self._motorId, msg.data)
self._output.write(req.dataTosend())
def __init__(self, param, output):
Device.__init__(self, param.getGpsName(), output)
self._frameId = param.getGpsFrameId()
self._pub = Publisher('%s' % self._name, NavSatFix, queue_size=param.getGpsPubHz())
self._haveRightToPublish = False
self._old_fix = 0
self._wd = int(round(time.time() * 1000))
KeepAliveHandler(self._name, NavSatFix)
def __init__(self, devId, param, output):
Device.__init__(self, param.getURFName(devId), output)
self._urfType = param.getURFType(devId)
self._frameId = param.getURFFrameId(devId)
self._pub = Publisher('%s' % self._name, Range, queue_size=param.getURFPubHz(devId))
#KeepAliveHandler(self._name, Range)
self._devId = devId
self._haveRightToPublish = False
class RiCServo(Device):
def __init__(self, params, servoNum, output):
Device.__init__(self, params.getServoName(servoNum), output)
self._servoNum = servoNum
self._pub = Publisher('%s/Position' % self._name, Float32, queue_size=params.getServoPublishHz(servoNum))
Subscriber('%s/command' % self._name, Float32, self.servoCallBack)
def publish(self, data):
msg = Float32()
msg.data = data
self._pub.publish(msg)
def servoCallBack(self, recv):
# TOOD: ServoRequest
position = recv.data
msg = ServoRequest(self._servoNum, position)
self._output.write(msg.dataTosend())
def main(self):
if len(sys.argv) >= 3:
rospy.init_node("RiC_PPM")
topicNamePmm = sys.argv[1]
topicNameDiff = sys.argv[2]
# print 'PPM Topic: ' + topicNamePmm + '\n' + 'Diff Topic: ' + topicNameDiff
self.pub = Publisher(topicNameDiff, Twist,queue_size=1)
Subscriber(topicNamePmm, PPM, self.callBack, queue_size=1)
rospy.spin()
def main_auto():
global read_yaw0, yaw_local
# initialize ROS node
init_node('auto_mode', anonymous=True)
ecu_pub = Publisher('ecu', ECU, queue_size = 10)
se_sub = Subscriber('imu/data', Imu, imu_callback)
# set node rate
rateHz = get_param("controller/rate")
rate = Rate(rateHz)
dt = 1.0 / rateHz
# get PID parameters
p = get_param("controller/p")
i = get_param("controller/i")
d = get_param("controller/d")
pid = PID(P=p, I=i, D=d)
setReference = False
# get experiment parameters
t_0 = get_param("controller/t_0") # time to start test
t_f = get_param("controller/t_f") # time to end test
FxR_target = get_param("controller/FxR_target")
t_params = (t_0, t_f, dt)
while not is_shutdown():
# OPEN LOOP
if read_yaw0:
# set reference angle
if not setReference:
pid.setPoint(yaw_local)
setReference = True
t_i = 0.0
# apply open loop command
else:
(FxR, d_f) = straight(t_i, pid, t_params, FxR_target)
ecu_cmd = ECU(FxR, d_f)
ecu_pub.publish(ecu_cmd)
t_i += dt
# wait
rate.sleep()
def __init__(self, param, output):
Device.__init__(self, param.getCloseDiffName(), output)
self._baseLink = param.getCloseDiffBaseLink()
self._odom = param.getCloseDiffOdom()
self._maxAng = param.getCloseDiffMaxAng()
self._maxLin = param.getCloseDiffMaxLin()
self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz())
self._broadCase = TransformBroadcaster()
Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1)
Service('%s/setOdometry' % self._name, set_odom, self.setOdom)
def arduino_interface():
# launch node, subscribe to motorPWM and servoPWM, publish ecu
init_node('arduino_interface')
llc = low_level_control()
Subscriber('ecu', ECU, llc.pwm_converter_callback, queue_size=1)
llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1)
# Set motor to neutral on shutdown
on_shutdown(llc.neutralize)
# process callbacks and keep alive
spin()
class TestHeadClient(unittest.TestCase):
def setUp(self):
rospy.init_node('effector_clients_test')
self.head_pub = Publisher('/mock/head', String)
self.client_instance = HeadClient()
self.effector_goal = MoveEndEffectorGoal()
self.effector_goal.command = 'TEST'
self.effector_goal.point_of_interest = '0,0,1'
rospy.sleep(1)
def test_init(self):
self.assertEqual(type(self.client_instance.client),
type(Client(move_head_topic, MoveSensorAction)))
# Following three must be in accordance to client_dict
self.assertEqual(type(self.client_instance.goal),
type(MoveSensorGoal()))
self.assertEqual(self.client_instance.topic, move_head_topic)
self.assertEqual(type(self.client_instance.action),
type(MoveSensorAction))
def test_fill_goal(self):
self.client_instance.fill_goal(self.effector_goal)
self.assertEqual(self.client_instance.goal.command, 0)
self.assertEqual(self.client_instance.goal.point_of_interest, '0,0,1')
def test_send_goal(self):
self.client_instance.wait_server()
self.client_instance.fill_goal(self.effector_goal)
self.client_instance.send_goal()
rospy.sleep(1)
self.assertEqual(self.client_instance.client.get_state(),
GoalStatus.ACTIVE)
def test_has_succeeded(self):
self.head_pub.publish(String('success:1'))
rospy.sleep(1)
self.client_instance.wait_server()
self.client_instance.fill_goal(self.effector_goal)
self.client_instance.send_goal()
self.client_instance.wait_result()
self.assertTrue(self.client_instance.has_succeeded())
def test_has_aborted(self):
self.head_pub.publish(String('abort:1'))
rospy.sleep(1)
self.client_instance.wait_server()
self.client_instance.fill_goal(self.effector_goal)
self.client_instance.send_goal()
self.client_instance.wait_result()
self.assertTrue(self.client_instance.has_aborted())
def test_has_preempted(self):
self.head_pub.publish(String('preempt:1'))
rospy.sleep(1)
self.client_instance.wait_server()
self.client_instance.fill_goal(self.effector_goal)
self.client_instance.send_goal()
self.client_instance.wait_result()
self.assertTrue(self.client_instance.has_preempted())
def arduino_interface():
global ecu_pub, motor_pwm, servo_pwm
init_node('arduino_interface')
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
time_prev = time.time()
ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10)
while not rospy.is_shutdown():
if time.time() >= time_prev and time.time() < time_prev + 2:
motor_pwm = 1580.0
elif time.time() < time_prev + 4:
motor_pwm = 1620.0
elif time.time() < time_prev + 6:
motor_pwm = 1500.0
elif time.time() < time_prev + 9:
motor_pwm = 1580.0
servo_pwm = 1200.0
elif time.time() < time_prev + 11:
motor_pwm = 1500.0
servo_pwm = 1500
elif time.time() < time_prev + 14:
motor_pwm = 1580.0
servo_pwm = 1800.0
elif time.time() < time_prev + 17:
motor_pwm = 1500.0
servo_pwm = 1500
elif time.time() >= time_prev + 18:
break
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
# wait
rate.sleep()
def step():
publisher = Publisher('turtle1/cmd_vel', Twist, queue_size = 10)
init_node('control')
rate = Rate(100)
while not is_shutdown():
keys = {
"fwd": get_param('fwd'),
"bwd": get_param('bwd'),
"rht": get_param('rht'),
"lft": get_param('lft')
}
system('clear')
display(keys)
move = get_move(keys)
if move == 'quit':
return True
break
if move is not None:
publisher.publish(move)
rate.sleep()
def main_auto():
global throttle, steering
# initialize the ROS node
init_node('manual_control', anonymous=True)
Subscriber('rc_inputs', ECU_PWM, rc_inputs_callback)
nh = Publisher('ecu_pwm', ECU_PWM, queue_size=1)
# set node rate
rateHz = 50
dt = 1.0 / rateHz
rate = Rate(rateHz)
throttle = 1500
steering = 1500
# main loop
while not is_shutdown():
ecu_cmd = ECU_PWM(throttle, steering)
nh.publish(ecu_cmd)
rospy.loginfo('ecu_pwm_pub send [%s]' % str(ecu_cmd))
rate.sleep()
def sonar():
pub = Publisher('sonar_meas', Float64WithHeader, queue_size=10)
pub1 = Publisher('sonar_meas_control', Float64, queue_size=10)
init_node('sonar', anonymous=True)
rate = Rate(20) # 10hz
while not is_shutdown():
GPIO.output(TRIG, True)
time.sleep(0.00001)
GPIO.output(TRIG, False)
count = 0
while GPIO.input(ECHO) == 0:
if count > 1000:
break
pulse_start = time.time()
count += 1
count = 0
while GPIO.input(ECHO) == 1:
if count > 1000:
break
pulse_end = time.time()
count += 1
pulse_duration = pulse_end - pulse_start
distance = pulse_duration * 17150
distance = distance / 100.0
sonar_data = Float64WithHeader()
sonar_data.header.stamp = get_rostime()
if abs(distance) < 2.0:
sonar_data.float.data = distance
pub1.publish(distance)
else:
sonar_data.float.data = 0.0
pub1.publish(0.0)
pub.publish(sonar_data)
rate.sleep()
def arduino_interface():
global ecu_pub
# launch node, subscribe to motorPWM and servoPWM, publish ecu
init_node('arduino_interface')
Subscriber('ecu', ECU, pwm_converter_callback, queue_size=1)
ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1)
# Set motor to neutral on shutdown
on_shutdown(neutralize)
# process callbacks and keep alive
spin()
class TestInitState(unittest.TestCase):
""" Tests for the init state. """
def setUp(self):
rospy.init_node('state_init_test')
self.effector_mock = Publisher('mock/effector', String)
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('exploration')
def test_init_to_exploration(self):
self.effector_mock.publish('success:1')
self.agent.to_init()
self.assertEqual(self.agent.state, 'exploration')
def test_initialization_with_effector_failure(self):
self.effector_mock.publish('abort:1')
# If the end effector is not responsive the init
# task will loop forever. Using this decorator
# we limit the execution time of the task.
# Wrap your function and test the wrapper.
@TimeLimiter(timeout=5)
def init_wrapper():
self.agent.to_init()
self.assertRaises(TimeoutException, init_wrapper)
self.effector_mock.publish('preempt:1')
@TimeLimiter(timeout=5)
def init_wrapper():
self.agent.to_init()
self.assertRaises(TimeoutException, init_wrapper)
def test_global_state_change(self):
self.effector_mock.publish('success:1')
initial_state = RobotModeMsg.MODE_OFF
final_state = RobotModeMsg.MODE_START_AUTONOMOUS
self.assertEqual(self.agent.state_changer.get_current_state(),
initial_state)
self.agent.wake_up()
self.assertEqual(self.agent.state_changer.get_current_state(),
final_state)
class RangePublisher(object):
"""
Base class for publising Range messages
"""
def __init__(self, name, fov, min_dist, max_dist, rad_type, queue_size):
self._name = name
self._range_msg = Range()
self._range_msg.radiation_type = rad_type
self._range_msg.field_of_view = fov
self._range_msg.min_range = min_dist
self._range_msg.max_range = max_dist
self._publisher = Publisher(name, Range, queue_size=queue_size)
def _msg(self, index, distance):
msg = self._range_msg
msg.header.frame_id = "{}_{}".format(self._name, index)
msg.header.stamp = Time.now()
msg.range = min(max(distance, msg.min_range), msg.max_range)
return msg
def publish(self, index, distance):
self._publisher.publish(self._msg(index, distance))
class Chaser:
def __init__(self, vel):
self.vel = vel
self.subsc1 = Subscriber('/turtle1/pose', Pose, self.get_pose1)
self.subsc2 = Subscriber('/turtle2/pose', Pose, self.get_pose2)
self.pub = Publisher('/turtle2/cmd_vel', Twist, queue_size=10)
self.pose2 = Pose()
def get_pose1(self, pose):
self.pose2 = pose
def get_pose2(self, pose):
new_angle = np.arctan2(x1=[self.pose2.y, pose.y],
x2=[self.pose2.x, pose.x])
while new_angle < -np.pi:
new_angle += 2 * np.pi
while new_angle > np.pi:
new_angle -= 2 * np.pi
msg = Twist()
msg.linear.x = self.vel
msg.angular.z = new_angle
self.pub.publish(msg)
def main():
init_node('state_publisher')
# Create publishers
armed_pub = Publisher('/dron_employee/armed', Bool, queue_size=1)
# TODO: Battery charger monitor thread
#ready_pub = Publisher('/dron_employee/battery_full', Bool, queue_size=1)
# State handler
def dron_sate(msg):
armed = Bool()
armed.data = msg.armed
armed_pub.publish(armed)
Subscriber('/mavros/state', State, dron_sate)
spin()
def setUp(self):
rospy.init_node('test_move_end_effector_server_node')
self.effector_client = Client(move_end_effector_controller_topic,
MoveEndEffectorAction)
self.server = MoveEndEffectorServer()
self.goal = MoveEndEffectorGoal()
self.goal_maker = EffectorGoalMaker()
self.sensor_pub = Publisher('mock/sensor', String)
self.linear_actuator_pub = Publisher('mock/linear_actuator', String)
self.head_pub = Publisher('mock/head', String)
rospy.sleep(1)
def wifi_signal_monitor():
init_node('bthere_wifi_signal_monitor', anonymous=False)
pub = Publisher('/bthere/wifi_signal', WifiData, queue_size=10)
loginfo('Outputting to /bthere/wifi_signal')
test_output = get_param('~test_output', False)
update_period = get_param('~update_period', 15.0)
quiet = get_param('~quiet', False)
rate = Rate(1 / float(update_period))
loginfo('Publishing rate: ' + str(1 / float(update_period)) + 'hz')
while not is_shutdown():
if (test_output):
output_test_data(rate, pub, quiet)
else:
output_wifi(rate, pub, quiet)
rate.sleep()
def random_sonar(delay=1):
print('Starting random sonar...')
pub = Publisher('/sensors/range', Range)
msg = Range()
while not rospy.is_shutdown():
msg.header = rospy.Header(frame_id='right_sonar_frame')
msg.range = random.random() * 20
pub.publish(msg)
sleep(delay)
msg.header = rospy.Header(frame_id='left_sonar_frame')
msg.range = random.random() * 20
pub.publish(msg)
class ProportionalDriveState(EventState):
"""
Drive directly to a target using a simple proportional controller.
># target_position PointStamped Point to drive to
<= ok Drive is complete
"""
def __init__(self):
super(ProportionalDriveState, self).__init__(outcomes=['ok'], input_keys=['target_position'])
self._twist_topic = '/drive_controller/cmd_vel'
self._odom_topic = '/ekf/odom/odometry/filtered'
self._visual_topic = '/viz/proportional_drive/target_position'
self._twist_publisher = Publisher(self._twist_topic, Twist, queue_size=1)
self._odometry = SubscriberValue(self._odom_topic, Odometry)
self._visual_publisher = Publisher(self._visual_topic, PointStamped, queue_size=1)
self._target_position = None # type: PointStamped
self.STOPPING_DISTANCE = 0.1
self.FORWARD_SPEED = 1.0
self.MAX_ANGULAR_SPEED = 6.28
self.PROPORTIONAL_CONSTANT = 6.0
def on_enter(self, userdata):
self._target_position = userdata.target_position
def execute(self, ud):
self._visual_publisher.publish(self._target_position)
odometry = self._odometry.value # type: Odometry
assert odometry.header.frame_id == self._target_position.header.frame_id
rover_position2d = numpify(odometry.pose.pose.position)[:2]
target_position2d = numpify(self._target_position.point)[:2]
if np.linalg.norm(rover_position2d - target_position2d) < self.STOPPING_DISTANCE:
return 'ok'
rover_angle = quaternion_to_yaw(odometry.pose.pose.orientation)
angle_to_target = np.arctan2(target_position2d[1] - rover_position2d[1], target_position2d[0] - rover_position2d[0])
diff = angular_diff(angle_to_target, rover_angle)
print(diff)
t = Twist()
if abs(diff) < np.pi / 4:
t.linear.x = self.FORWARD_SPEED
t.angular.z = diff * self.PROPORTIONAL_CONSTANT
if abs(t.angular.z) > self.MAX_ANGULAR_SPEED:
t.angular.z = self.MAX_ANGULAR_SPEED * np.sign(t.angular.z)
self._twist_publisher.publish(t)
def on_exit(self, userdata):
self._twist_publisher.publish(Twist())
def testStatusPubWorker2(self, sleep, publish, shutdown):
TestCloudUploader.rospy_is_shutdown_first_run = True
cu = CloudUploader()
cu.status_publisher = Publisher("/test",
UploadStatusList,
queue_size=0)
cu.queue.put(CloudUploader.UploadTask("topic", "value", "filename"))
cu.queue.put(CloudUploader.UploadTask("topic", "value", "filename2"))
cu.currentTask = None
cu.status_pub_worker()
assert shutdown.call_count == 2
publish.assert_called_once()
assert len(publish.call_args[0][0].statusList) == 2
assert publish.call_args[0][0].statusList[0].file_name == "filename"
assert publish.call_args[0][0].statusList[0].uploading == False
assert publish.call_args[0][0].statusList[1].file_name == "filename2"
assert publish.call_args[0][0].statusList[1].uploading == False
sleep.assert_called_once()
def __init__(self,
id,
name,
initial_value,
ideal_value,
upper_limit,
lower_limit,
satisfaction_time,
params_std,
evolving,
time_step=1.0):
self.__name = name
self.__id = id
# Evolution Parameters
self.__initial_value = initial_value
self.__current_value = initial_value
self.__ideal_value = ideal_value
self.__upper_limit = upper_limit
self.__lower_limit = lower_limit
self.__satisfaction_time = satisfaction_time
self.__params_std = params_std
self.__time_step = time_step
# Initializes Evolution
self.__evolving = evolving
self.__evol_params = params_std
self.__punctual = False
self.__stop = False
self.__no_saturation = 0.1 # Prevents saturation
# Publisher
self.__pub = Publisher(name.lower() + "/value",
Float32,
latch=True,
queue_size=1)
# Defines evolving method
self.__std_evol = self.set_evolution(params_std["te_id"])
# Creates thread and lock
Thread.__init__(self, target=self.__std_evol)
self.__lock = Lock()
def arduino_interface():
global ecu_pub, b0
# launch node, subscribe to motorPWM and servoPWM, publish ecu
init_node('arduino_interface')
b0 = get_param("input_gain")
Subscriber(
'ecu', ECU, pwm_converter_callback,
queue_size=10) # queue_size set to 1 (instead of 10) in Vercantez/barc
ecu_pub = Publisher(
'ecu_pwm', ECU,
queue_size=10) # queue_size set to 1 (instead of 10) in Vercantez/barc
# Set motor to neutral on shutdown
on_shutdown(neutralize)
# process callbacks and keep alive
spin()
def main():
''' The main routine '''
init_node('quad_flight_estimation')
pub = Publisher('/path_estimation/cost', PathCost, queue_size=1)
def cost_estimation(msg):
# Calc path length in kilometers
length = path_length(msg.base.latitude, msg.base.longitude,
msg.destination.latitude, msg.destination.longitude)
print('Path length is {0} km'.format(length))
# Calc cost by path length
cost = get_cost_by(length)
print('Cost is {0} szabo'.format(cost))
# Make message and publish
cost_msg = PathCost()
cost_msg.ident = msg.ident
cost_msg.cost = cost
pub.publish(cost_msg)
Subscriber('/path_estimation/path', PathEstimate, cost_estimation)
spin()
class TestExplorer(unittest.TestCase):
""" Tests for the explorer action client """
def setUp(self):
# Register the mock servers.
rospy.init_node('unit_explorer')
self.explorer_mock = Publisher('mock/explorer', String)
self.agent = Agent(strategy='normal')
def test_explore(self):
self.explorer_mock.publish('success:4')
self.agent.explore()
self.assertTrue(self.agent.explorer.exploration_pending.is_set())
self.assertEqual(self.agent.explorer.client.get_state(),
GoalStatus.PENDING)
def test_preempt(self):
self.explorer_mock.publish('success:3')
self.agent.explore()
self.agent.preempt_explorer()
self.assertFalse(self.agent.explorer.exploration_pending.is_set())
def test_explorer_abort(self):
self.explorer_mock.publish('abort:1')
self.agent.explore()
sleep(3)
self.assertEqual(self.agent.explorer.client.get_state(),
GoalStatus.ABORTED)
self.assertFalse(self.agent.explorer.exploration_pending.is_set())
def test_explorer_success(self):
self.explorer_mock.publish('success:1')
self.agent.explore()
sleep(3)
self.assertEqual(self.agent.explorer.client.get_state(),
GoalStatus.SUCCEEDED)
self.assertFalse(self.agent.explorer.exploration_pending.is_set())
def _create_publishers():
pubs = dict()
pubs["workspace"] = Publisher("/workspace",
Marker,
queue_size=1,
latch=True)
pubs["tsdf"] = Publisher("/tsdf", PointCloud2, queue_size=1, latch=True)
pubs["points"] = Publisher("/points",
PointCloud2,
queue_size=1,
latch=True)
pubs["quality"] = Publisher("/quality",
PointCloud2,
queue_size=1,
latch=True)
pubs["grasp"] = Publisher("/grasp", MarkerArray, queue_size=1, latch=True)
pubs["grasps"] = Publisher("/grasps",
MarkerArray,
queue_size=1,
latch=True)
pubs["debug"] = Publisher("/debug", PointCloud2, queue_size=1, latch=True)
return pubs
class TestNavigator(unittest.TestCase):
""" Tests for the base action client """
def setUp(self):
# Register the mock servers.
rospy.init_node('unit_base_control')
self.move_base_mock = Publisher('mock/feedback_move_base', String)
self.agent = Agent(strategy='normal')
target = mock_msgs.create_victim_info(id=8, probability=0.65)
self.agent.target_victim = target
def test_move_base_abort(self):
if not rospy.is_shutdown():
sleep(1)
self.move_base_mock.publish('abort:1')
random_pose = mock_msgs.create_pose_stamped(x=1, y=2, z=3)
self.agent.navigator.move_base(random_pose)
self.agent.navigator.client.wait_for_result()
self.assertEqual(self.agent.navigator.client.get_state(),
GoalStatus.ABORTED)
self.assertFalse(self.agent.navigator.base_pending.is_set())
def test_move_base_success(self):
if not rospy.is_shutdown():
sleep(1)
self.move_base_mock.publish('success:1')
random_pose = mock_msgs.create_pose_stamped(x=1, y=2, z=3)
self.agent.navigator.move_base(random_pose)
self.agent.navigator.client.wait_for_result()
self.assertEqual(self.agent.navigator.client.get_state(),
GoalStatus.SUCCEEDED)
self.assertFalse(self.agent.navigator.base_pending.is_set())
def test_move_base_preempt(self):
if not rospy.is_shutdown():
sleep(1)
self.move_base_mock.publish('success:1')
random_pose = mock_msgs.create_pose_stamped(x=1, y=2, z=3)
self.agent.navigator.move_base(random_pose)
self.agent.navigator.cancel_all_goals()
self.assertFalse(self.agent.navigator.base_pending.is_set())
def __init__(self, param, output):
Device.__init__(self, param.getIMUName(), output)
self._frameId = param.getIMUFrameId()
self._angle = param.getIMUOrientation()
self._pub = Publisher('%s_AGQ' % self._name,
Imu,
queue_size=param.getIMUPubHz())
self._pubMag = Publisher('%s_M' % self._name,
MagneticField,
queue_size=param.getIMUPubHz())
self._pubCalib = Publisher('/imu_calib_publisher',
imuCalib,
queue_size=param.getIMUPubHz())
Service('/imu_Calibration', calibIMU, self.serviceCallBack)
self._haveRightToPublish = False
self._calib = False
self._xMax = 0.0
self._yMax = 0.0
self._zMax = 0.0
self._xMin = 0.0
self._yMin = 0.0
self._zMin = 0.0
class ControlAuthorityServer():
def __init__(self):
self.authority = False
control_authority_srv = Service('/dji_sdk/sdk_control_authority',
SDKControlAuthority, self.handler)
self.control_pub = Publisher('/dji_sdk/matlab_rpvy', Joy, queue_size=1)
self.wind_pub = Publisher('/dji_sdk/matlab_wind', Joy, queue_size=1)
joy_sub = Subscriber('/dji_sdk/xbox_joy', Joy, self.joy_callback)
control_sub = Subscriber(
'/dji_sdk/flight_control_setpoint_rollpitch_yawrate_zvelocity',
Joy, self.control_callback)
def handler(self, request):
self.authority = request.control_enable
loginfo('authority = %s', self.authority)
return SDKControlAuthorityResponse(True, 0, 0, 0)
def joy_callback(self, msg):
a = msg.axes
if msg.buttons[5] == 1:
wind = Joy(axes=[5 * a[3], 5 * a[2], 5 * a[1], 2 * a[0]])
self.wind_pub.publish(wind)
else:
if self.authority:
self.authority = False
loginfo('authority = %s', self.authority)
joy = Joy(axes=[-a[2], a[3], 2 * a[1], 2 * a[0]])
self.control_pub.publish(joy)
def control_callback(self, msg):
if self.authority:
self.control_pub.publish(msg)
def arduino_interface():
global ecu_pub, motor_pwm, servo_pwm
init_node('arduino_interface')
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
time_prev = time.time()
ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10)
motor_pwm = 1600
servo_pwm = 1500
flag = 0
while not rospy.is_shutdown():
if time.time() - time_prev >= 3:
servo_pwm = 1300
motor_pwm = 1650
if time.time() - time_prev >= 5:
servo_pwm = 1500
if time.time() - time_prev >= 7:
servo_pwm = 1700
motor_pwm = 1600
if time.time() - time_prev >= 9:
servo_pwm = 1500
motor_pwm = 1500
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
break
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
# wait
rate.sleep()
if len(element.strip().strip("'")) > 0
]
RosbridgeTcpSocket.services_glob = [
element.strip().strip("'")
for element in get_param('~services_glob', '')[1:-1].split(',')
if len(element.strip().strip("'")) > 0
]
RosbridgeTcpSocket.params_glob = [
element.strip().strip("'")
for element in get_param('~params_glob', '')[1:-1].split(',')
if len(element.strip().strip("'")) > 0
]
# Publisher for number of connected clients
RosbridgeTcpSocket.client_count_pub = Publisher('client_count',
Int32,
queue_size=10,
latch=True)
RosbridgeTcpSocket.client_count_pub.publish(0)
# update parameters if provided via commandline
# .. could implemented 'better' (value/type checking, etc.. )
if "--port" in sys.argv:
idx = sys.argv.index("--port") + 1
if idx < len(sys.argv):
port = int(sys.argv[idx])
else:
print("--port argument provided without a value.")
sys.exit(-1)
if "--host" in sys.argv:
idx = sys.argv.index("--host") + 1
def cpu_monitor():
"""Publishes CPU data to /bthere/cpu_data."""
architecture = uname()[
4] # This will return 'x86_64', 'aarc64' (for 64 bit arm), etc.
if (not architecture in SUPPORTED_ARCHITECTURES):
logerr(
"This architecture doesn't appear to be one that is supported. Consider adding it and openning"
+ " a pull request on github!")
exit()
init_node("bthere_cpu_monitor", anonymous=False)
pub = Publisher("/bthere/cpu_data", CPUData, queue_size=10)
loginfo("Outputting to /bthere/cpu_data")
#update period should to be somewhat small since the cpu load data is average since you last checked,
#a slower update rate will be less accurate for bursty loads and may introduce more lag than expected
#if a load is added later in the time between updates for example.
update_period = get_param('~update_period', 1.0)
rate = Rate(1 / float(update_period))
loginfo("Publishing rate: " + str(1.0 / update_period) + " hz")
quiet = get_param("~quiet", False)
#since the temperature-getting seems likely to be failure prone, try it once to check.
able_to_get_temps = True
if (isnan(get_cpu_temps(architecture)[0])):
logwarn("Unable to get CPU temperatures")
able_to_get_temps = False
last_cpu_times = []
while not is_shutdown():
data = CPUData()
gated_loginfo(quiet, "------ CPU Data ------")
if (able_to_get_temps):
# If temperature data can be collected, add it to the CPUData to be published and log
package_temp, core_temps = get_cpu_temps(architecture)
gated_loginfo(quiet, "CPU Package temp. (C): " + str(package_temp))
data.package_temp = package_temp
if (len(core_temps) > 0):
for core in range(len(core_temps)):
gated_loginfo(
quiet, "CPU Core " + str(core) + "temp. (C): " +
str(core_temps[core]))
data.core_temps = core_temps
else:
# If the data is unavailable just publish NaN and log
gated_loginfo(quiet, "CPU temperatures unavailable")
data.package_temp = float("NaN")
data.core_temps = [float("NaN")]
if (len(last_cpu_times) == 0):
# If this hasn't been initialized, we just won't publish this info yet and init.
# last_cpu_times can't just be initialized before the loop because it should (for consistency) be the same
# time between data collections and getting the initial data before the loop would make the time between
# data collections small and potentially make the data misleading due to burst loads.
last_cpu_times = get_load_data()
gated_loginfo(quiet, "CPU load not yet available")
else:
overall_load, per_cores, last_cpu_times = get_cpu_load(
last_cpu_times)
gated_loginfo(
quiet,
"Overall CPU load: " + str(round(overall_load * 100, 1)) + "%")
data.overall_cpu_load = overall_load
if (len(per_cores) > 0):
for core in range(len(per_cores)):
gated_loginfo(
quiet, "CPU core " + str(core) + " load: " +
str(round(per_cores[core] * 100, 1)) + "%")
data.core_loads = per_cores
# Add the header information:
header = Header(stamp=Time.now())
# The frame_id property seems to be to do with tf frames of reference. That isn't useful for something like
# this, so just leave it empty. (this might be the wrong way to do this, but I don't have any other info.)
# The sequential id is apparently set by the publisher.
data.header = header
pub.publish(data)
rate.sleep()
def __init__(self, mode_type_change_handler, traffic_light_change_handler, world_image_change_handler, color_peak_change_handler):
#
# Publishers
#
self.mode_change_pub = Publisher(
"/smart_home/mode_change_chatter",
ModeChange,
queue_size=1
)
self.device_activation_change_pub = Publisher(
"/smart_home/device_activation_change_chatter",
DeviceActivationChange,
queue_size=MAX_AUX_DEVICE_COUNT
)
self.intensity_change_pub = Publisher(
"/smart_home/intensity_change_chatter",
Float32,
queue_size=1
)
self.countdown_state_pub = Publisher(
"/smart_home/countdown_state_chatter",
CountdownState,
queue_size=1
)
self.active_frequencies_pub = Publisher(
"/smart_home/active_frequencies_chatter",
Float32Arr,
queue_size=1
)
self.start_wave_mode_pub = Publisher(
"/smart_home/start_wave_mode_chatter",
Empty,
queue_size=1
)
self.wave_update_pub = Publisher(
"/smart_home/wave_update_chatter",
WaveUpdate,
queue_size=MAX_AUX_DEVICE_COUNT
)
#
# Subscribers
#
self.mode_change_request_sub = Subscriber(
"/smart_home/mode_change_request_chatter",
ModeChangeRequest,
self.mode_change_request_callback,
queue_size=1
)
self.countdown_state_sub = Subscriber(
"/smart_home/countdown_state_chatter",
CountdownState,
self.countdown_state_callback,
queue_size=1
)
self.participant_location_sub = Subscriber(
"/smart_home/wave_participant_location_chatter",
WaveParticipantLocation,
self.participant_location_callback,
queue_size=MAX_AUX_DEVICE_COUNT
)
self.color_peak_left_sub = Subscriber(
"/smart_home/color_peak_left",
Color,
lambda msg: color_peak_change_handler(msg, 1),
queue_size=1
)
self.color_peak_right_sub = Subscriber(
"/smart_home/color_peak_right",
Color,
lambda msg: color_peak_change_handler(msg, 2),
queue_size=1
)
self.cap_peaks_telem_sub = Subscriber(
"/smart_home/color_peaks_telem",
ColorPeaksTelem,
self.color_peaks_telem_callback,
queue_size=1
)
#
# Service clients
#
self.screen_calibration_request_cli = ServiceProxy(
"/smart_home/screen_calibration_requests",
RequestScreenCalibration
)
self.screen_calibration_set_homography_points_cli = ServiceProxy(
"/smart_home/screen_calibration_homography_points",
SetScreenCalibrationPointsOfHomography
)
#
# Other initialization
#
self.mode_type_change_handler = mode_type_change_handler
self.traffic_light_change_handler = traffic_light_change_handler
self.world_image_change_handler = world_image_change_handler
self.current_mode = None
self.active_mode_sequence_characteristics = (-1,)
self.send_mode_type(ModeChange.FULL_OFF, False)
self.participant_locations = None
SmartHomeHubNodeInterface.log_info("Started.")
class SmartHomeHubNodeInterface(object):
## The constructor. Makes the ROS connections (publishers, subscriptions, etc.) and initializes the
# mode type.
#
# @param self The object pointer.
# @param mode_type_change_handler The function callback for the GUI when a request to change the
# mode type is sent.
# @param traffic_light_change_handler The function callback for the GUI when a the state of the
# traffic light is changed.
# @param world_image_change_handler The function callback to update the screen in world frame.
def __init__(self, mode_type_change_handler, traffic_light_change_handler, world_image_change_handler, color_peak_change_handler):
#
# Publishers
#
self.mode_change_pub = Publisher(
"/smart_home/mode_change_chatter",
ModeChange,
queue_size=1
)
self.device_activation_change_pub = Publisher(
"/smart_home/device_activation_change_chatter",
DeviceActivationChange,
queue_size=MAX_AUX_DEVICE_COUNT
)
self.intensity_change_pub = Publisher(
"/smart_home/intensity_change_chatter",
Float32,
queue_size=1
)
self.countdown_state_pub = Publisher(
"/smart_home/countdown_state_chatter",
CountdownState,
queue_size=1
)
self.active_frequencies_pub = Publisher(
"/smart_home/active_frequencies_chatter",
Float32Arr,
queue_size=1
)
self.start_wave_mode_pub = Publisher(
"/smart_home/start_wave_mode_chatter",
Empty,
queue_size=1
)
self.wave_update_pub = Publisher(
"/smart_home/wave_update_chatter",
WaveUpdate,
queue_size=MAX_AUX_DEVICE_COUNT
)
#
# Subscribers
#
self.mode_change_request_sub = Subscriber(
"/smart_home/mode_change_request_chatter",
ModeChangeRequest,
self.mode_change_request_callback,
queue_size=1
)
self.countdown_state_sub = Subscriber(
"/smart_home/countdown_state_chatter",
CountdownState,
self.countdown_state_callback,
queue_size=1
)
self.participant_location_sub = Subscriber(
"/smart_home/wave_participant_location_chatter",
WaveParticipantLocation,
self.participant_location_callback,
queue_size=MAX_AUX_DEVICE_COUNT
)
self.color_peak_left_sub = Subscriber(
"/smart_home/color_peak_left",
Color,
lambda msg: color_peak_change_handler(msg, 1),
queue_size=1
)
self.color_peak_right_sub = Subscriber(
"/smart_home/color_peak_right",
Color,
lambda msg: color_peak_change_handler(msg, 2),
queue_size=1
)
self.cap_peaks_telem_sub = Subscriber(
"/smart_home/color_peaks_telem",
ColorPeaksTelem,
self.color_peaks_telem_callback,
queue_size=1
)
#
# Service clients
#
self.screen_calibration_request_cli = ServiceProxy(
"/smart_home/screen_calibration_requests",
RequestScreenCalibration
)
self.screen_calibration_set_homography_points_cli = ServiceProxy(
"/smart_home/screen_calibration_homography_points",
SetScreenCalibrationPointsOfHomography
)
#
# Other initialization
#
self.mode_type_change_handler = mode_type_change_handler
self.traffic_light_change_handler = traffic_light_change_handler
self.world_image_change_handler = world_image_change_handler
self.current_mode = None
self.active_mode_sequence_characteristics = (-1,)
self.send_mode_type(ModeChange.FULL_OFF, False)
self.participant_locations = None
SmartHomeHubNodeInterface.log_info("Started.")
## Log info to ROSOUT.
#
# @param smsg The string message to log.
@staticmethod
def log_info(smsg):
loginfo(smsg)
## Log debug to ROSOUT.
#
# @param smsg The string message to log.
@staticmethod
def log_debug(smsg):
logdebug(smsg)
## Log warn to ROSOUT.
#
# @param smsg The string message to log.
@staticmethod
def log_warn(smsg):
logwarn(smsg)
## Log err to ROSOUT.
#
# @param smsg The string message to log.
@staticmethod
def log_err(smsg):
logerr(smsg)
## Log fatal to ROSOUT.
#
# @param smsg The string message to log.
@staticmethod
def log_fatal(smsg):
logfatal(smsg)
## Repackages and sends out the requested mode type.
#
# @param self The object pointer.
# @param msg The ROS message describing the node change request.
def mode_change_request_callback(self, msg):
self.send_mode_type(msg.mode_type, True)
## A handler for countdown state changes. The data is sent back to the GUI to set its traffic light.
#
# @param self The object pointer.
# @param msg The ROS message describing the updated countdown state.
def countdown_state_callback(self, msg):
self.traffic_light_change_handler(msg.state)
## A handler for a wave mode participant's response.
#
# @param self The object pointer.
# @param msg The ROS message describing an appliance participating in the wave along with its location.
def participant_location_callback(self, msg):
if self.participant_locations is not None:
self.participant_locations.append((msg.participant_id, msg.position))
## A handler for color peak process telemetry.
#
# @param self The object pointer.
# @param msg The ROS message with the telemetry info.
def color_peaks_telem_callback(self, msg):
self.world_image_change_handler(msg)
## The routine to take the provided mode type ROS constant and send a new message.
#
# @param self The object pointer.
# @param mode_type The new mode type.
# @param call_handler Whether or not to call the appropriate handler from the GUI.
def send_mode_type(self, mode_type, call_handler):
mode_change_msg = ModeChange()
mode_change_msg.mode_type = mode_type
mode_change_msg.header.stamp = get_rostime()
self.mode_change_pub.publish(mode_change_msg)
self.current_mode = mode_type
SmartHomeHubNodeInterface.log_info("Set mode to [{0}].".format(self.current_mode))
if call_handler:
self.mode_type_change_handler()
if mode_type == ModeChange.WAVE:
self.participant_locations = []
self.start_wave_mode_pub.publish(Empty())
else:
self.participant_locations = None
## A helper function to package a device (in)activation request.
#
# @param self The object pointer.
# @param device_id The coordinated ID for the device that is being requested to be activated or deactivated.
# @param active Whether or not the device is to be activated or otherwise.
def send_device_activation_change(self, device_id, active):
device_activation_change_msg = DeviceActivationChange()
device_activation_change_msg.device_id = device_id
device_activation_change_msg.active = active
self.device_activation_change_pub.publish(device_activation_change_msg)
SmartHomeHubNodeInterface.log_info("Set device with ID [{0}] to [{1}ACTIVE].".format(device_id, "" if active else "IN"))
## A helper function to package the batched intensity and publish it to the ROS network.
#
# @param self The object pointer.
# @param intensity The normalized intensity to send.
def send_intensity_change(self, intensity):
intensity_change_msg = Float32()
intensity_change_msg.data = intensity
self.intensity_change_pub.publish(intensity_change_msg)
SmartHomeHubNodeInterface.log_info("Set intensity to [{0}].".format(intensity))
## A helper function to take a requested state and send it out to the ROS network.
#
# @param self The object pointer.
# @param state The requested countdown state.
def send_countdown_state(self, state):
countdown_state_msg = CountdownState()
countdown_state_msg.state = state
self.countdown_state_pub.publish(countdown_state_msg)
SmartHomeHubNodeInterface.log_info("Set countdown state to [{0}].".format(state))
## A helper function to package the batched wave period and publish it to the ROS network.
#
# @param self The object pointer.
# @param id_intensity_pairs A list of tuples of size 2, containing the participant IDs and their
# corresponding intensities.
def send_wave_update(self, id_intensity_pairs):
wave_update_msg = WaveUpdate()
for pair in id_intensity_pairs:
wave_update_msg.participant_ids.append(pair[0])
wave_update_msg.intensities.data.append(pair[1])
self.wave_update_pub.publish(wave_update_msg)
## Issue a service request to get currently isolated corners.
#
# @param self The object pointer.
# @param max_corners The max corners to find.
# @param quality_level The minimal acceptable quality of corners.
# @param min_dist Minimum possible Euclidean pixel distance between found corners.
def request_screen_calibration(self, max_corners, quality_level, min_dist):
return self.screen_calibration_request_cli(
max_corners=max_corners,
quality_level=quality_level,
min_dist=min_dist,
do_blur=True,
kh=5,
kw=5
)
def confirm_screen_calibration(self, pts):
return self.screen_calibration_set_homography_points_cli(
pts_of_homog=[Point(x=x,y=y) for y,x in pts]
)
def __init__(self):
init_node('iiwa_sunrise', log_level=INFO)
hardware_interface = get_param('~hardware_interface',
'PositionJointInterface')
self.robot_name = get_param('~robot_name', 'iiwa')
model = get_param('~model', 'iiwa14')
tool_length = get_param('~tool_length', 0.0)
if model == 'iiwa7':
self.l02 = 0.34
self.l24 = 0.4
elif model == 'iiwa14':
self.l02 = 0.36
self.l24 = 0.42
else:
logerr('unknown robot model')
return
self.l46 = 0.4
self.l6E = 0.126 + tool_length
self.tr = 0.0
self.v = 1.0
self.joint_names = [
'{}_joint_1'.format(self.robot_name), '{}_joint_2'.format(
self.robot_name), '{}_joint_3'.format(self.robot_name),
'{}_joint_4'.format(self.robot_name),
'{}_joint_5'.format(self.robot_name),
'{}_joint_6'.format(self.robot_name),
'{}_joint_7'.format(self.robot_name)
]
joint_states_sub = Subscriber('joint_states',
JointState,
self.jointStatesCb,
queue_size=1)
command_pose_sub = Subscriber('command/CartesianPose',
PoseStamped,
self.commandPoseCb,
queue_size=1)
command_pose_lin_sub = Subscriber('command/CartesianPoseLin',
PoseStamped,
self.commandPoseLinCb,
queue_size=1)
redundancy_sub = Subscriber('command/redundancy',
Float64,
self.redundancyCb,
queue_size=1)
joint_position_sub = Subscriber('command/JointPosition',
JointPosition,
self.jointPositionCb,
queue_size=1)
self.state_pose_pub = Publisher('state/CartesianPose',
PoseStamped,
queue_size=1)
self.joint_trajectory_pub = Publisher(
'{}_trajectory_controller/command'.format(hardware_interface),
JointTrajectory,
queue_size=1)
path_parameters_configuration_srv = Service(
'configuration/setSmartServoLimits', SetSmartServoJointSpeedLimits,
self.handlePathParametersConfiguration)
path_parameters_lin_configuration_srv = Service(
'configuration/setSmartServoLinLimits',
SetSmartServoLinSpeedLimits,
self.handlePathParametersLinConfiguration)
smart_servo_configuration_srv = Service(
'configuration/ConfigureControlMode', ConfigureControlMode,
self.handleSmartServoConfiguration)
spin()
