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 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 publish_image_as_pointcloud(point_cloud_publisher: rospy.Publisher,
message: PointCloud2):
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
message.header = h
message.header.frame_id = 'img2rviz'
point_cloud_publisher.publish(message)
def arduino_interface():
global ecu_pub, motor_pwm, servo_pwm
# initialize node
init_node('arduino_interface')
# setup publishers and subscribers
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)
v_meas_pub = rospy.Publisher('v_meas', Float32, queue_size=10)
rospy.Subscriber('encoder', Encoder, enc_callback)
while not rospy.is_shutdown():
if time.time() >= time_prev and time.time() < time_prev + 4:
motor_pwm = 1650
if time.time() >= time_prev + 4:
motor_pwm = 1500
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
v_meas_pub.publish(Float32(v_meas))
# wait
rate.sleep()
class Topic(object):
def __init__(self, name, message, publisher_queue=100):
self.message = message
self.name = resolve_name(name)
self.publisher = Publisher(self.name,
self.message,
queue_size=publisher_queue)
self.subscribers = {}
def __del__(self):
self.close()
def close(self):
self.publisher.unregister()
for (handler, subscriber) in self.subscribers.items():
subscriber.unregister()
def publish(self, message):
self.publisher.publish(message)
def subscribe(self, handler):
self.subscribers[handler] = Subscriber(self.name, self.message,
handler)
def unsubscribe(self, handler):
del self.subscribers[handler]
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 + 4:
motor_pwm = 1580.0
if time.time() >= time_prev + 4 and time.time() < time_prev + 6:
motor_pwm = 1620.0
if time.time() >= time_prev + 6 and time.time() < time_prev + 8:
motor_pwm = 1600.0
if time.time() >= time_prev + 8 and time.time() < time_prev + 10:
motor_pwm = 1500.0
if time.time() >= time_prev + 10:
break
ecu_cmd = ECU(motor_pwm, servo_pwm)
ecu_pub.publish(ecu_cmd)
# wait
rate.sleep()
class RoutePlanner:
def __init__(self):
self.pub = Pub('/clicked_point', Point, queue_size=100)
def _publish_point(self, x, y):
sleep(0.5)
p = Point()
p.header.frame_id = 'map'
p.point.x = float(x)
p.point.y = float(y)
self.pub.publish(p)
def go(self, x, y):
self._publish_point(x - 1, y - 1)
self._publish_point(x + 1, y - 1)
self._publish_point(x, y + 1)
self._publish_point(x - 1, y - 1)
self._publish_point(x, y)
def explore(self):
self._publish_point(-100, -100)
self._publish_point(-100, 100)
self._publish_point(100, 100)
self._publish_point(100, -100)
self._publish_point(-100, -100)
self._publish_point(0, 0)
class Chase:
def __init__(self):
self.position = Pose()
self.publisher = Publisher('/turtle2/cmd_vel', Twist, queue_size=10)
self.sub_t1 = Subscriber('/turtle1/pose', Pose, self.chasing)
self.sub_t2 = Subscriber('/turtle2/pose', Pose, self.pose_upd)
@staticmethod
def calc_distance(p_0, p_1):
return sqrt((p_1.y - p_0.y) ** 2 + (p_1.x - p_0.x) ** 2)
@staticmethod
def calc_angle(p_0, p_1):
return atan2(p_1.y - p_0.y, p_1.x - p_0.x) - p_0.theta
def pose_upd(self, position):
self.position = position
def chasing(self, position):
twist = Twist()
distance = self.calc_distance(self.position, position)
if distance > 1e-1:
angle = self.calc_angle(self.position, position)
while angle > pi:
angle -= 2 * pi
while angle < - pi:
angle += 2 * pi
twist.linear.x = distance
twist.angular.z = angle
self.publisher.publish(twist)
class Estimator(object):
"""
Estimator Node: publishes object_id estimations once a second.
It subscribes to the accumulated predictions topic and produces estimations
of the object_id each second from these accumulated predictions.
"""
def __init__(self):
"""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...")
# Publishers and Subscribers
Subscriber("predictions", Predictions, self.callback)
self.pub = Publisher('final_object_id', SystemOutput)
def callback(self, predictions):
"""Callback that publishes updated predictions when new msg is recv."""
y, y_rgb, y_pcloud = alu.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 sendMockForces():
def __init__(self):
init_node("mock_forces", anonymous=True)
self._getParameters()
self._publisher = Publisher(self._outputTopic, OmniFeedback , queue_size=100)
def _getParameters(self):
self._outputTopic = get_param("~OutputTopic")
def run(self):
rosRate = Rate(30)
while not is_shutdown():
message = self._setForces()
loginfo(message)
self._publisher.publish(message)
rosRate.sleep()
def _setForces(self):
return OmniFeedback( force=Vector3(0,1,0),
position=Vector3(0,0,0) )
class TwistState(State):
def __init__(self, linear_vel: float, angular_vel: float):
super().__init__(outcomes=['ok', 'preempted'])
self.cmd_vel_pub = Publisher(CMD_VEL_TOPIC, Twist, queue_size=1)
self.rate = Rate(10)
self.duration = Duration(1)
self.linear_vel = linear_vel
self.angular_vel = angular_vel
def execute(self, ud):
start_time = Time.now()
while True:
if rospy.is_shutdown():
return 'preempted'
if self.preempt_requested():
self.service_preempt()
return 'preempted'
if Time.now() - start_time >= self.duration:
return 'ok'
self.cmd_vel_pub.publish(self.command())
self.rate.sleep()
def command(self):
twist = Twist()
twist.linear.x = self.linear_vel
twist.angular.z = self.angular_vel
return twist
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 = 1400
flag = 0
while not rospy.is_shutdown():
# if time.time()-time_prev>=3:
# servo_pwm = 1540
if time.time()-time_prev >= 8:
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()
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 WorldModelPub(object):
""" Mock publisher for the world_model. """
def __init__(self):
self._pub = Publisher(topics.world_model, WorldModel)
def send_random(self, duration=3, new_victims=2, old_victims=5):
msg = WorldModel()
msg.victims = [msgs.create_victim_info() for i in range(new_victims)]
msg.visitedVictims = [
msgs.create_victim_info() for i in range(old_victims)
]
for i in range(duration):
if not rospy.is_shutdown():
self._pub.publish(msg)
sleep(1)
def send_custom(self, victims, visited, duration=10):
msg = WorldModel()
msg.victims = victims
msg.visitedVictims = visited
for i in range(duration):
if not rospy.is_shutdown():
self._pub.publish(msg)
sleep(1)
def publish_message(self):
import rospy
from rospy import Publisher
from geometry_msgs.msg import Twist
rospy.init_node('move')
rospy.loginfo("About to be moving!")
publisher = Publisher('mobile_base/commands/velocity', Twist)
twist = Twist()
while True:
rospy.loginfo("Current/Desired/Step Linear Speed: " +
str(twist.linear.x) + "/" + str(self.linear.value) +
"/" + str(self.LINEAR_STEP_SPEED))
rospy.loginfo("Current/Desired/Step Angular Speed: " +
str(twist.angular.z) + "/" +
str(self.angular.value) + "/" +
str(self.ANGULAR_STEP_SPEED))
twist.linear.x = Driver.acceleration(
current_speed=twist.linear.x,
desired_speed=self.linear.value,
step_speed=self.LINEAR_STEP_SPEED)
twist.angular.z = Driver.acceleration(
current_speed=twist.angular.z,
desired_speed=self.angular.value,
step_speed=self.ANGULAR_STEP_SPEED)
publisher.publish(twist)
rospy.sleep(0.1)
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
#KeepAliveHandler(self._name, PPM)
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 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 Follower:
def __init__(self):
self.pub2 = Publisher('/leo/cmd_vel', Twist, queue_size=10)
self.sub2 = Subscriber('/leo/pose', Pose, self.update)
self.sub1 = Subscriber('/turtle1/pose', Pose, self.handle)
self.pose = Pose()
def update(self, my_pose):
self.pose = my_pose
def handle(self, pose):
msg = Twist()
dist = np.sqrt((pose.y - self.pose.y)**2 + (pose.x - self.pose.x)**2)
eps = 1e-4
new_theta = math.atan2(pose.y - self.pose.y, pose.x - self.pose.x)
ang = (new_theta - self.pose.theta)
while ang > np.pi:
ang -= 2 * np.pi
while ang < -np.pi:
ang += 2 * np.pi
msg.linear.x = min(c1 * dist, c3)
msg.angular.z = c2 * ang
if dist > eps:
self.pub2.publish(msg)
class TeleopTankDrive():
def __init__(self, max_vel):
self.vel_publisher = Publisher("/holonomic_drive_controller/command",
Float64MultiArray,
queue_size=10)
rospy.init_node("drive_velocity_setpoint", anonymous=True)
self.xbox_sub = rospy.Subscriber("/xbox_axes",
Float64MultiArray,
callback=self.set_velocity)
self.max_vel = max_vel
rospy.spin()
def set_velocity(self, axes):
linear_vel = axes.data[1]
angular_vel = axes.data[3]
left, right = self.calc_arcade_drive(linear_vel, angular_vel)
left = left * self.max_vel
right = right * self.max_vel
vels = Float64MultiArray()
vels.data = [left, left, -right, -right]
self.vel_publisher.publish(vels)
def calc_arcade_drive(self, linear, angular):
linear = math.copysign(linear * linear, linear)
angular = math.copysign(angular * angular, angular)
return (linear + angular, linear - angular)
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 TestVictimDeletionState(unittest.TestCase):
""" Tests for the victim deletion state. """
def setUp(self):
rospy.init_node('state_victim_deletion_test')
self.delete_victim_mock = Publisher('mock/delete_victim', String)
self.agent = Agent(strategy='normal')
self.agent.set_breakpoint('exploration')
def test_delete_victim_success(self):
target = mock_msgs.create_victim_info(id=1, probability=0.7)
self.agent.available_targets = [target]
self.agent.target.set(target)
if not rospy.is_shutdown():
sleep(1)
self.delete_victim_mock.publish('success:1')
self.agent.to_victim_deletion()
self.assertEqual(self.agent.state, 'exploration')
self.assertTrue(self.agent.target.is_empty)
self.assertIsNot(self.agent.available_targets, [])
def test_delete_victim_fail(self):
target = mock_msgs.create_victim_info(id=1, probability=0.7)
self.agent.available_targets = [target]
self.agent.target.set(target)
if not rospy.is_shutdown():
sleep(1)
self.delete_victim_mock.publish('abort:1')
self.agent.to_victim_deletion()
self.assertEqual(self.agent.state, 'exploration')
self.assertTrue(self.agent.target.is_empty)
self.assertIsNot(self.agent.available_targets, [])
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
# KeepAliveHandler(self._name, Bool)
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
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 Follower:
def __init__(self):
self.pub_2 = Publisher('/leo/cmd_vel', Twist, queue_size=10)
self.sub_1 = Subscriber('/turtle1/pose', Pose, self.follow)
self.sub_2 = Subscriber('/leo/pose', Pose, self.update)
self.pose = Pose()
self.eps = 1
def update(self, my_pose):
self.pose = my_pose
def follow(self, pose):
msg = Twist()
dist = np.sqrt((pose.y - self.pose.y)**2 + (pose.x - self.pose.x)**2)
if dist <= self.eps:
return
angle = (math.atan2(pose.y - self.pose.y, pose.x - self.pose.x) -
self.pose.theta)
while angle > np.pi:
angle -= 2 * np.pi
while angle < -np.pi:
angle += 2 * np.pi
msg.linear.x = min(dist, 1.0)
msg.angular.z = angle
self.pub_2.publish(msg)
class Turtle_follower:
def __init__(self):
self.subscriber_turtle1 = Subscriber('/turtle1/pose', Pose,
self.follow)
self.publisher = Publisher('/leo/cmd_vel', Twist, queue_size=10)
self.subscriber_leo = Subscriber('/leo/pose', Pose, self.update_pose)
self.pose = Pose()
def update_pose(self, pose):
self.pose = pose
def new_ang(self, new_theta):
ang = new_theta - self.pose.theta
while ang > np.pi:
ang -= 2 * np.pi
while ang < -np.pi:
ang += 2 * np.pi
return ang
def follow(self, pose):
msg = Twist()
p1, p2 = np.array([pose.x,
pose.y]), np.array([self.pose.x, self.pose.y])
distance = np.linalg.norm(p1 - p2)
if distance > 1:
new_theta = math.atan2(pose.y - self.pose.y, pose.x - self.pose.x)
msg.linear.x = min(distance, 2)
msg.angular.z = self.new_ang(new_theta)
self.publisher.publish(msg)
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 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
class VelocityControl():
def __init__(self):
self.Tc = CompactTransform.fromXYZRPY(0.0, 0.0, 0.0, 0.0, 0.0,
-20 * RAD2DEG)
self.got_pose = False
self.got_reference = False
self.velocity_pub = Publisher(
'/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1)
joy_sub = Subscriber('/joy', Joy, self.joy_callback)
pose_sub = Subscriber('/mavros/vision_pose/pose', PoseStamped,
self.pose_callback)
reference_sub = Subscriber('/mavros/control/reference', PoseStamped,
self.reference_callback)
def pose_callback(self, msg):
self.TBW = CompactTransform.fromPose(msg.pose)
self.got_pose = True
loginfo_once('got pose')
def reference_callback(self, msg):
self.TRW = CompactTransform.fromPose(msg.pose)
self.got_reference = True
loginfo_once('got reference')
def joy_callback(self, msg):
a = msg.axes
#self.velocity_pub.publish(
#PoseStamped(
#header = Header(
#frame_id = 'world'),
#pose = Pose(
#position = Point(
#x = a[3],
#y = a[2],
#z = a[1]),
#orientation = Quaternion(
#x = 0.0,
#y = 0.0,
#z = 0.0,
#w = 1.0))))
if a[4] < -0.5 and self.got_pose and self.got_reference:
TBRW = self.TRW - self.TBW
TBRWc = self.Tc * TBRW
vr = saturate(TBRWc.p, 1.0)
wr = saturate(TBRW.r(), 1.0)
vx = vr[0]
vy = vr[1]
vz = vr[2]
wz = wr[2]
else:
vx = a[3]
vy = a[2]
vz = a[1]
wz = a[0]
self.velocity_pub.publish(
Twist(linear=Vector3(x=vx, y=vy, z=vz),
angular=Vector3(x=0.0, y=0.0, z=wz)))
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 + 6.5:
motor_pwm = 1650
if time.time() >= time_prev + 6.5:
motor_pwm = 1500 #1465
#if time.time() >= time_prev + 5 and time.time() < time_prev + 10:
# motor_pwm = 84.0
#if time.time() >= time_prev + 12 and time.time() < time_prev + 17:
# motor_pwm = 86.0
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()
class OdometryPublisher:
def __init__(self,
frame="odom",
child_frame="base_footprint",
queue_size=5):
self._frame_id = frame
self._child_frame_id = child_frame
self._publisher = Publisher("odom", Odometry, queue_size=queue_size)
self._broadcaster = TransformBroadcaster()
self._odom_msg = Odometry()
self._odom_msg.header.frame_id = self._frame_id
self._odom_msg.child_frame_id = self._child_frame_id
def _msg(self, now, x, y, quat, v, w):
"""
Updates the Odometry message
:param now: Current time
:param x_dist: Current x distance traveled
:param y_dist: Current y distance traveled
:param quat: Quaternion of the orientation
:param v: Linear velocity
:param w: Angular velocity
:return: Odometry message
"""
self._odom_msg.header.stamp = now
self._odom_msg.pose.pose.position.x = x
self._odom_msg.pose.pose.position.y = y
self._odom_msg.pose.pose.position.z = 0
self._odom_msg.pose.pose.orientation = quat
self._odom_msg.twist.twist = Twist(Vector3(v, 0, 0), Vector3(0, 0, w))
return self._odom_msg
def publish(self, now, x, y, v, w, heading, log=False):
"""
Publishes an Odometry message
:param cdist: Center (or average) distance of wheel base
:param v: Linear velocity
:param w: Angular velocity
:param heading: Heading (or yaw)
:param quat: Quaternion of orientation
:return: None
"""
quat = Quaternion()
quat.x = 0.0
quat.y = 0.0
quat.z = math.sin(heading / 2)
quat.w = math.cos(heading / 2)
self._broadcaster.sendTransform((x, y, 0),
(quat.x, quat.y, quat.z, quat.w), now,
self._child_frame_id, self._frame_id)
self._publisher.publish(self._msg(now, x, y, quat, v, w))
if log:
loginfo(
"Publishing Odometry: heading {:02.3f}, dist {:02.3f}, velocity {:02.3f}/{:02.3f}"
.format(heading, math.sqrt(x**2 + y**2), v, w))
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)
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)
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)
def visualize_point_cloud(pub: rospy.Publisher, pc: np.ndarray):
list_of_tuples = [tuple(point) for point in pc]
dtype = [('x', np.float32), ('y', np.float32), ('z', np.float32)]
np_record_array = np.array(list_of_tuples, dtype=dtype)
msg = ros_numpy.msgify(PointCloud2,
np_record_array,
frame_id='world',
stamp=rospy.Time.now())
pub.publish(msg)
def send_gripper_command(command_pub: rospy.Publisher, positions):
if positions is not None:
cmd = default_gripper_command()
cmd.header.stamp = rospy.Time.now()
cmd.finger_a_command.position = positions[0]
cmd.finger_b_command.position = positions[1]
cmd.finger_c_command.position = positions[2]
cmd.scissor_command.position = positions[3]
command_pub.publish(cmd)
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)
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)
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_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)
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
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())
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_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()
class RiCURF(Device):
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
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)
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(self.getType(), self._devId, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and subCheck == 'None':
self._output.write(PublishRequest(self.getType(), self._devId, False).dataTosend())
self._haveRightToPublish = False
except: pass
def main_auto():
global throttle, steering
# initialize the ROS node
init_node('manual_control', anonymous=True)
Subscriber('rc_inputs', ECU, rc_inputs_callback)
nh = Publisher('ecu_pwm', ECU, queue_size = 10)
# set node rate
rateHz = 50
dt = 1.0/rateHz
rate = Rate(rateHz)
throttle = 90
steering = 90
# main loop
while not is_shutdown():
ecu_cmd = ECU(throttle, steering)
nh.publish(ecu_cmd)
rate.sleep()
class RiCIMU(Device):
def __init__(self,param ,output):
Device.__init__(self, param.getIMUName(), output)
self._frameId = param.getIMUFrameId()
self._pub = Publisher('%s_AGQ' % self._name, Imu, queue_size=param.getIMUPubHz())
self._pubMag = Publisher('%s_M' % self._name, MagneticField, queue_size=param.getIMUPubHz())
def publish(self, data):
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = data.getOrientation()
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
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)
self._haveRightToPublish = False
def publish(self, data):
msg = Float32()
msg.data = data
self._pub.publish(msg)
def servoCallBack(self, recv):
Thread(target=self.sendMsg, args=(recv,)).start()
# TOOD: ServoRequest
def getType(self): return SERVO
def sendMsg(self, recv):
position = recv.data
msg = ServoRequest(self._servoNum, position)
self._output.write(msg.dataTosend())
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(SERVO, self._servoNum, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and subCheck == 'None':
self._output.write(PublishRequest(SERVO, self._servoNum, False).dataTosend())
self._haveRightToPublish = False
except: pass
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())
def publish(self, data):
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()
if data.getFix() == 1:
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)
class RiCIMU(Device):
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
#KeepAliveHandler('%s_AGQ' % self._name, Imu)
def getType(self):
return IMU
def serviceCallBack(self, request):
validStatus = True
if request.status == calibIMURequest.START_CALIB:
rospy.loginfo("Calibration begin")
self._output.write(IMURequest(calibIMURequest.START_CALIB).dataTosend())
self._calib = True
self._xMax = request.xMax
self._yMax = request.yMax
self._zMax = request.zMax
self._xMin = request.xMin
self._yMin = request.yMin
self._zMin = request.zMin
elif request.status == calibIMURequest.STOP_CALIB:
rospy.loginfo("Calibration end")
self._output.write(IMURequest(calibIMURequest.STOP_CALIB).dataTosend())
rospy.loginfo("Calibration setting saved")
self._calib = False
else:
validStatus = False
return {'ack': validStatus}
def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion((q.y, q.z, q.w, q.x))
#array = quaternion_from_euler(yaw + (self._angle * pi / 180), roll, pitch)
#array = quaternion_from_euler(yaw + (self._angle * pi / 180), -1 (roll - 180), -1 * pitch)
array = quaternion_from_euler((yaw + (self._angle * pi / 180)), roll, -1 * pitch)
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
def checkForSubscribers(self):
try:
subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]
subMagCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pubMag.name)).group(0).split(': ')[1]
subCalibCheck = \
re.search('Subscribers:.*', rostopic.get_info_text(self._pubCalib.name)).group(0).split(': ')[1]
if not self._haveRightToPublish and (subCheck == '' or subMagCheck == '' or subCalibCheck == ''):
self._output.write(PublishRequest(IMU, 0, True).dataTosend())
self._haveRightToPublish = True
elif self._haveRightToPublish and (
subCheck == 'None' and subMagCheck == 'None' and subCalibCheck == 'None'):
self._output.write(PublishRequest(IMU, 0, False).dataTosend())
self._haveRightToPublish = False
except:
pass
class MultiPublisher():
""" Keeps track of the clients that are using a particular publisher.
Provides an API to publish messages and register clients that are using
this publisher """
def __init__(self, topic, msg_type=None, latched_client_id=None, queue_size=100):
""" Register a publisher on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the publisher to
msg_type -- (optional) the type to register the publisher as. If not
provided, an attempt will be made to infer the topic type
latch -- (optional) if a client requested this publisher to be latched,
provide the client_id of that client here
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topic_type = get_topic_type(topic)[0]
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
if topic_type is not None and topic_type != msg_class._type:
raise TypeConflictException(topic, topic_type, msg_class._type)
# Create the publisher and associated member variables
self.clients = {}
self.latched_client_id = latched_client_id
self.topic = topic
self.msg_class = msg_class
self.publisher = Publisher(topic, msg_class, latch=(latched_client_id!=None), queue_size=queue_size)
self.listener = PublisherConsistencyListener()
self.listener.attach(self.publisher)
def unregister(self):
""" Unregisters the publisher and clears the clients """
self.publisher.unregister()
self.clients.clear()
def verify_type(self, msg_type):
""" Verify that the publisher publishes messages of the specified type.
Keyword arguments:
msg_type -- the type to check this publisher against
Throws:
Exception -- if ros_loader cannot load the specified msg type
TypeConflictException -- if the msg_type is different than the type of
this publisher
"""
if not ros_loader.get_message_class(msg_type) is self.msg_class:
raise TypeConflictException(self.topic,
self.msg_class._type, msg_type)
return
def publish(self, msg):
""" Publish a message using this publisher.
Keyword arguments:
msg -- the dict (json) message to publish
Throws:
Exception -- propagates exceptions from message conversion if the
provided msg does not properly conform to the message type of this
publisher
"""
# First, check the publisher consistency listener to see if it's done
if self.listener.attached and self.listener.timed_out():
self.listener.detach()
# Create a message instance
inst = self.msg_class()
# Populate the instance, propagating any exceptions that may be thrown
message_conversion.populate_instance(msg, inst)
# Publish the message
self.publisher.publish(inst)
def register_client(self, client_id):
""" Register the specified client as a client of this publisher.
Keyword arguments:
client_id -- the ID of the client using the publisher
"""
self.clients[client_id] = True
def unregister_client(self, client_id):
""" Unregister the specified client from this publisher.
If the specified client_id is not a client of this publisher, nothing
happens.
Keyword arguments:
client_id -- the ID of the client to remove
"""
if client_id in self.clients:
del self.clients[client_id]
def has_clients(self):
""" Return true if there are clients to this publisher. """
return len(self.clients) != 0
