def work():
global lock
global coverages
lock.acquire()
total = sum(coverages.values())
lock.release()
pub.publish(sms.Float64(total))
def set_height(self, h):
f = stdm.Float64()
f.data = h
self.controller_pub.publish(f)
rospy.loginfo("%s: starting traj", self.controller_name)
self.pr2.start_thread(JustWaitThread(0.5))
#self.goto_joint_positions([h])
#self.torso_client.send_goal(pcm.SingleJointPositionGoal(position = h))
#self.torso_client.wait_for_result() # todo: actually check result
return
def send_to_all(self, msg):
for pub in self.publishers.values():
pub.publish(stdMsg.Float64(msg))
# if __name__ == '__main__':
# node = ComanPublisher()
# if len(sys.argv) > 1:
# r = rospy.Rate(10) # 10hz
# else:
# r = rospy.Rate(sys.argv[1])
# rospy.loginfo("Running coman publisher...")
# while not rospy.is_shutdown():
# # Send zeros to coman
# node.send_to_all(np.zeros(29))
# r.sleep()
def _point_cloud_callback(self, data):
points = [
p for p in data.points
if abs(math.atan(p.y / p.x)) < self._view_angle / 2
]
def function(parameters):
wall_distance, slope = parameters
return [
((wall_distance + slope * p.y) - p.x) / math.sqrt(1 + slope**2)
for p in points
]
result = scipy.optimize.leastsq(function, [1, 0])
error = sum(function(result[0]))
if error > 1e-4:
rospy.logwarn('High wall angle error: %r' % error)
cloud_angle_message = std_msgs.Float64()
cloud_angle_message.data = math.atan(result[0][1])
self._publisher.publish(cloud_angle_message)
def send(self, msgs={}):
for k, v in msgs.items():
self.publishers[k].publish(stdMsg.Float64(v))
def create_state(self):
"""Uses data from :py:attr:`recent` to create the state representation.
1. Reads data from the :py:attr:`recent` dictionary.
2. Process data into a format to pass to :doc:`state_representation`.
3. Pass data to :py:func:`~state_representation.StateManager.get_phi`
and :py:func:`~state_representation.StateManager.get_observation`.
Returns:
(numpy array, dict): Feature vector and ancillary state
information.
"""
# bumper constants from
# http://docs.ros.org/hydro/api/kobuki_msgs/html/msg/SensorState.html
bump_codes = [1, 4, 2]
# initialize data
additional_features = set(tools.features.keys() + ['charging'])
sensors = self.features_to_use.union(additional_features)
# read data (used to make phi)
data = {sensor: None for sensor in sensors}
for source in sensors - {'ir', 'core'}:
data[source] = self.read_source(source)
data['ir'] = self.read_source('ir', history=True)[-10:]
data['core'] = self.read_source('core', history=True)
# process data
if data['core']:
bumps = [dat.bumper for dat in data['core']]
data['bump'] = np.sum(
[[bool(x & bump) for x in bump_codes] for bump in bumps],
axis=0, dtype=bool).tolist()
data['charging'] = bool(data['core'][-1].charger & 2)
# enter the data into rosbag
if self.COLLECT_DATA_FLAG:
for bindex in range(len(data['bump'])):
bump_bool = std_msg.Bool()
bump_bool.data = data['bump'][bindex] if data['bump'][
bindex] else False
self.history.write('bump' + str(bindex), bump_bool,
t=self.current_time)
charge_bool = std_msg.Bool()
charge_bool.data = data['charging']
self.history.write('charging', charge_bool,
t=self.current_time)
if data['ir']:
ir = [[0] * 6] * 3
# bitwise 'or' of all the ir data in last time_step
for temp in data['ir']:
a = [[int(x) for x in format(temp, '#08b')[2:]] for temp in
[ord(obs) for obs in temp.data]]
ir = [[k | l for k, l in zip(i, j)] for i, j in zip(a, ir)]
data['ir'] = [int(''.join([str(i) for i in ir_temp]), 2) for
ir_temp in ir]
# enter the data into rosbag
if self.COLLECT_DATA_FLAG:
ir_array = std_msg.Int32MultiArray()
ir_array.data = data['ir']
self.history.write('ir', ir_array, t=self.current_time)
if data['image'] is not None:
# enter the data into rosbag
# image_array = std_msg.Int32MultiArray()
# image_array.data = data['image']
if self.COLLECT_DATA_FLAG:
self.history.write('image', data['image'], t=self.current_time)
# uncompressed image
data['image'] = np.fromstring(data['image'].data,
np.uint8).reshape(480, 640, 3)
# compressing image
if data['cimage'] is not None:
data['image'] = cv2.imdecode(np.fromstring(data['cimage'].data,
np.uint8),
1)
if data['odom'] is not None:
pos = data['odom'].pose.pose.position
lin_vel = data['odom'].twist.twist.linear.x
ang_vel = data['odom'].twist.twist.angular.z
data['odom'] = np.array([pos.x, pos.y, lin_vel, ang_vel])
# enter the data into rosbag
if self.COLLECT_DATA_FLAG:
odom_x = std_msg.Float64()
odom_x.data = pos.x
odom_y = std_msg.Float64()
odom_y.data = pos.y
odom_lin = std_msg.Float64()
odom_lin.data = lin_vel
odom_ang = std_msg.Float64()
odom_ang.data = ang_vel
self.history.write('odom_x', odom_x, t=self.current_time)
self.history.write('odom_y', odom_y, t=self.current_time)
self.history.write('odom_lin', odom_lin, t=self.current_time)
self.history.write('odom_ang', odom_ang, t=self.current_time)
if data['imu'] is not None:
data['imu'] = data['imu'].orientation.z
# TODO: enter the data into rosbag
if 'bias' in self.features_to_use:
data['bias'] = True
data['weights'] = self.gvfs[0].learner.theta if self.gvfs else None
phi = self.state_manager.get_phi(**data)
if 'last_action' in self.features_to_use:
last_action = np.zeros(self.behavior_policy.action_space.size)
last_action[self.behavior_policy.last_index] = True
phi = np.concatenate([phi, last_action])
# update the visualization of the image data
if self.vis:
self.visualization.update_colours(data['image'])
observation = self.state_manager.get_observations(**data)
observation['action'] = self.last_action
if observation['bump']:
# adds a tally for the added cumulant
self.cumulant_counter.value += 1
return phi, observation
def __init__(self):
rospy.init_node('problem3')
rospy.wait_for_service('problem1a')
self.p1 = rospy.ServiceProxy('problem1a', p4_srv.PositionBucket)
rospy.wait_for_service('problem2a')
self.p2a = rospy.ServiceProxy('problem2a', p4_srv.FindPath)
rospy.wait_for_service('problem2b')
self.p2b = rospy.ServiceProxy('problem2b', p4_srv.FollowPath)
rospy.wait_for_service('/vrep/bucket/pos/get')
self.s = rospy.ServiceProxy('/vrep/bucket/pos/get', p4_srv.GetPosition)
rospy.wait_for_service('/vrep/block1/pos/get')
self.b1 = rospy.ServiceProxy('/vrep/block1/pos/get',
p4_srv.GetPosition)
rospy.wait_for_service('/vrep/block2/pos/get')
self.b2 = rospy.ServiceProxy('/vrep/block2/pos/get',
p4_srv.GetPosition)
rospy.wait_for_service('/vrep/block3/pos/get')
self.b3 = rospy.ServiceProxy('/vrep/block3/pos/get',
p4_srv.GetPosition)
rospy.wait_for_service('/vrep/block4/pos/get')
self.b4 = rospy.ServiceProxy('/vrep/block4/pos/get',
p4_srv.GetPosition)
rospy.wait_for_service('/vrep/block5/pos/get')
self.b5 = rospy.ServiceProxy('/vrep/block5/pos/get',
p4_srv.GetPosition)
rospy.wait_for_service('/vrep/youbot/arm/reach')
self.reach = rospy.ServiceProxy('/vrep/youbot/arm/reach',
p4_srv.ReachPos)
rospy.wait_for_service('/vrep/youbot/gripper/grip')
self.grip = rospy.ServiceProxy('/vrep/youbot/gripper/grip',
std_srvs.SetBool)
self.setangle1 = rospy.Publisher('/vrep/youbot/arm/joint1/angle',
std_msgs.Float64,
queue_size=10)
self.setangle2 = rospy.Publisher('/vrep/youbot/arm/joint2/angle',
std_msgs.Float64,
queue_size=10)
self.setangle3 = rospy.Publisher('/vrep/youbot/arm/joint3/angle',
std_msgs.Float64,
queue_size=10)
self.setangle4 = rospy.Publisher('/vrep/youbot/arm/joint4/angle',
std_msgs.Float64,
queue_size=10)
self.setangle5 = rospy.Publisher('/vrep/youbot/arm/joint5/angle',
std_msgs.Float64,
queue_size=10)
self.vel_publisher = rospy.Publisher('/vrep/youbot/base/cmd_vel',
Twist,
queue_size=10)
self.rate = rospy.Rate(10)
#print("ini done")
#pass
#def callback(self):
#print("enter call")
b = [self.b1(), self.b2(), self.b3(), self.b4(), self.b5()]
self.p1()
for i in range(1, 6):
start = rospy.wait_for_message('/vrep/youbot/base/pose',
Pose).position
goal = b[i - 1].pos #from current position to block
num_waypoints = 3
path = self.p2a(start, goal, num_waypoints).path
self.p2b(path)
pos = goal
pos.z += 0.03
diff = 1
while diff > 1e-6:
gripos1 = rospy.wait_for_message(
'/vrep/youbot/arm/gripper/pose', Pose).position
self.reach(pos).success
gripos2 = rospy.wait_for_message(
'/vrep/youbot/arm/gripper/pose', Pose).position
diff = gripos2.z - gripos1.z
data = True
times = 0
while self.grip(data).success and times < 10:
times += 1
self.grip(data)
self.setangle1.publish(std_msgs.Float64(0))
self.setangle2.publish(std_msgs.Float64(0))
self.setangle3.publish(std_msgs.Float64(0))
self.setangle4.publish(std_msgs.Float64(0))
self.setangle5.publish(std_msgs.Float64(0))
start = rospy.wait_for_message('/vrep/youbot/base/pose',
Pose).position
goal = self.s().pos #from current position to bucket
num_waypoints = 3
path = self.p2a(start, goal, num_waypoints).path
self.p2b(path)
pos = goal
diff = 1
while diff > 1e-6:
gripos1 = rospy.wait_for_message(
'/vrep/youbot/arm/gripper/pose', Pose).position
self.reach(pos).success
gripos2 = rospy.wait_for_message(
'/vrep/youbot/arm/gripper/pose', Pose).position
diff = gripos2.z - gripos1.z
data = False
times = 0
while self.grip(data).success and times < 10:
times += 1
self.grip(data)
self.grip(data)
self.setangle1.publish(std_msgs.Float64(0))
self.setangle2.publish(std_msgs.Float64(0))
self.setangle3.publish(std_msgs.Float64(0))
self.setangle4.publish(std_msgs.Float64(0))
self.setangle5.publish(std_msgs.Float64(0))
def main():
global G_pv
# SOME BASIC CONSTANTS
gearSign = 1 # Use to set calibration HOME direction
depth_min = 0.0010
depth_max = 0.0145
# Absolute magnitude of stopping point randomization
# so we don't stop on exactly the same tooth each time.
# 0.00325/15 = 0.0002167 = 1 motor rev (with 15 & 3.25mm pitch)
epsilon_mag = 0.0002167 / 2
# PARSE INPUT!
# THEY MIGHT BE ASKING FOR -h HELP, SO DON'T START ROS YET.
parser = OptionParser()
parser.add_option("-n",
dest="num_cycles",
metavar="num_cycles",
type="int",
default=1000,
help="Number of cycles")
parser.add_option("-a",
dest="depth_min",
metavar="min_depth",
type="float",
default=depth_min,
help="Min depth for cycles. (default is %4.1f mm)" %
(1000 * depth_min))
parser.add_option("-d",
dest="depth_max",
metavar="max_depth",
type="float",
default=depth_max,
help="Max depth for cycles. (default is %4.1f mm)" %
(1000 * depth_max))
parser.add_option("-t",
dest="cycletime",
metavar="cycletime",
type="float",
default=6.0,
help="Time for one cycle")
parser.add_option("-v",
dest="verbose",
action="store_true",
default=False,
help="Flag for verbose output")
(options, args) = parser.parse_args()
# STANDARDIZE INPUT sign AND NORMALIZE TO SI UNITS
depth_min = max(abs(options.depth_min), 2 * epsilon_mag)
if depth_min > 0.020: depth_min /= 1000.0
depth_max = abs(options.depth_max)
if depth_max > 0.020: depth_max /= 1000.0
# CHECK FOR roscore AND cycle_controller
rosInfra()
# NOW SETUP ROS NODE
joint = "gripper_joint"
rospy.init_node('cycle', anonymous=True)
pub = rospy.Publisher("%s/command" % CONTROLLER_NAME, msg_std.Float64)
sub_pv = rospy.Subscriber('cycle_controller/state',
msg_ctrlr.JointControllerState, cb_pv)
# ECHO OPERATING PARAMETERS AFTER STARTING NODE
print("")
print "num_cycles = %d" % options.num_cycles
print "min_depth = %4.1f mm" % (1000 * depth_min)
print "max_depth = %4.1f mm" % (1000 * depth_max)
print "cycletime = %4.1f sec" % options.cycletime
goal = depth_min
for n in range(2 * options.num_cycles):
if options.verbose: G_pv = 3
else: G_pv = 2
rospy.sleep(0.010) # ALLOW Callback TO PRINT
epsilon = epsilon_mag * (2 * random.random() - 1)
pubGoal = gearSign * abs(goal)
pub.publish(msg_std.Float64(pubGoal))
if goal != depth_min:
goal = depth_min
sys.stdout.write("\rCycle # %4d goal= %7.4lf mm " %
(n / 2 + 1, abs(pubGoal)))
sys.stdout.flush()
else:
goal = depth_max
sys.stdout.write("\rCycle # %4d goal= %7.4lf mm " %
(n / 2 + 1, abs(pubGoal)))
sys.stdout.flush()
rospy.sleep(options.cycletime / 2.0)
if rospy.is_shutdown():
break
pub.publish(msg_std.Float64(gearSign * abs(depth_min)))
print("\n")
