def callback(msg):
global last_time, yaw, pub, last_yaw, count, pub_pose
now = msg.header.stamp
if last_time == None:
last_time = now
yaw = 0
last_yaw = 0
count = 0
return
count += 1
q = msg.orientation
delta_yaw = efq((q.x, q.y, q.z, q.w))[2] * (now - last_time).to_sec()
# delta_yaw = efq((q.x, q.y, q.z, q.w))[2] * 1.0/200.0
yaw += delta_yaw
last_time = now
if count != 10:
return
count = 0
delta_yaw = last_yaw - yaw
last_yaw = yaw
covar = 1.0/abs(radians(delta_yaw))
print delta_yaw, radians(delta_yaw), covar
q = qfe(0, 0, radians(yaw))
header = msg.header
msg = PoseStamped()
msg.header = header
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1]
msg.pose.orientation.z = q[2]
msg.pose.orientation.w = q[3]
pub_pose.publish(msg)
msg = Imu()
msg.header = header
msg.orientation.x = q[0]
msg.orientation.y = q[1]
msg.orientation.z = q[2]
msg.orientation.w = q[3]
msg.orientation_covariance = [1e+100, 0, 0,
0, 1e+100, 0,
0, 0, covar]
pub.publish(msg)
def face_callback(self, msg):
if not self.found_face:
face = PointStamped()
face.header = msg.people[0].header
face.point = msg.people[0].pos
self.face_parent_frame = msg.people[0].header.frame_id
# self.listener.waitForTransform(face.header.frame_id, 'base_link', rospy.Time.now(), rospy.Duration(5.0))
d = sqrt(face.point.x * face.point.x + face.point.y * face.point.y)
# Change the axes from camera-type axes
self.quaternion = quaternion_from_euler(pi/2, pi/2, 0.0)
pose = PoseStamped()
pose.header = face.header
pose.pose.position = face.point
pose.pose.orientation.x = self.quaternion[0]
pose.pose.orientation.y = self.quaternion[1]
pose.pose.orientation.z = self.quaternion[2]
pose.pose.orientation.w = self.quaternion[3]
# Transform to base_link
# pose = self.listener.transformPose('base_link', pose)
face = pose.pose.position
self.quaternion = (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w)
self.origin = (face.x, face.y, face.z)
# Flip the bit
self.found_face = True
def callback(self, torus_msg, coef_msg):
if coef_msg.header.frame_id != torus_msg.header.frame_id:
rospy.logerr('frame_id is not correct. coef: {0}, torus: {1}'.format(
coef_msg.header.frame_id, torus_msg.header.frame_id))
# convert torus_msg.pose to matrix
torus_rot = quaternion_matrix([torus_msg.pose.orientation.x,
torus_msg.pose.orientation.y,
torus_msg.pose.orientation.z,
torus_msg.pose.orientation.w])
# 3x3 x 3x1
torus_z_axis = np.dot(torus_rot[:3, :3], np.array([0, 0, 1]))
coef_axis = np.array([coef_msg.coefficients[0].values[0],
coef_msg.coefficients[0].values[1],
coef_msg.coefficients[0].values[2]])
torus_z_axis = torus_z_axis / np.linalg.norm(torus_z_axis)
coef_axis = coef_axis / np.linalg.norm(coef_axis)
theta = acos(np.dot(torus_z_axis, coef_axis))
rospy.loginfo("theta: {0} rad({1} deg)".format(theta, degrees(theta)))
if theta > self.eps_angle:
rospy.logwarn("torus detection is rejected")
else:
rospy.loginfo("torus detection is allowed")
self.pub_torus.publish(torus_msg)
arr = TorusArray()
arr.header = torus_msg.header
arr.toruses = [torus_msg]
self.pub_torus_array.publish(arr)
pose_stamped = PoseStamped()
pose_stamped.header = torus_msg.header
pose_stamped.pose = torus_msg.pose
self.pub_pose.publish(pose_stamped)
def move_cb(self, msg):
fprint("Move request received!", msg_color="blue")
if msg.move_type not in ['hold', 'drive', 'skid', 'circle']:
fprint("Move type '{}' not found".format(msg.move_type), msg_color='red')
self.move_server.set_aborted(MoveResult('move_type'))
return
self.blind = msg.blind
p = PoseStamped()
p.header = make_header(frame="enu")
p.pose = msg.goal
self.goal_pose_pub.publish(p)
# Sleep before you continue
rospy.sleep(1)
yaw = trns.euler_from_quaternion(rosmsg_to_numpy(msg.goal.orientation))[2]
if not self.is_feasible(np.array([msg.goal.position.x, msg.goal.position.y, yaw]), np.zeros(3)):
fprint("Not feasible", msg_color='red')
self.move_server.set_aborted(MoveResult('occupied'))
return
fprint("Finished move!", newline=2)
self.move_server.set_succeeded(MoveResult(''))
def in_odom_callback(self, in_odom_msg):
q = np.array([in_odom_msg.pose.pose.orientation.x,
in_odom_msg.pose.pose.orientation.y,
in_odom_msg.pose.pose.orientation.z,
in_odom_msg.pose.pose.orientation.w])
p = np.array([in_odom_msg.pose.pose.position.x,
in_odom_msg.pose.pose.position.y,
in_odom_msg.pose.pose.position.z])
e = tfs.euler_from_quaternion(q, 'rzyx')
wqb = tfs.quaternion_from_euler(e[0], e[1], e[2], 'rzyx')
wqc = tfs.quaternion_from_euler(e[0], 0.0, 0.0, 'rzyx')
#### odom ####
odom_msg = in_odom_msg
assert(in_odom_msg.header.frame_id == self.frame_id_in)
odom_msg.header.frame_id = self.frame_id_out
odom_msg.child_frame_id = ""
self.out_odom_pub.publish(odom_msg)
#### tf ####
if self.broadcast_tf and self.tf_pub_flag:
self.tf_pub_flag = False
if not self.frame_id_in == self.frame_id_out:
br.sendTransform((0.0, 0.0, 0.0),
tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'),
odom_msg.header.stamp,
self.frame_id_in,
self.frame_id_out)
if not self.world_frame_id == self.frame_id_out:
br.sendTransform((0.0, 0.0, 0.0),
tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'),
odom_msg.header.stamp,
self.world_frame_id,
self.frame_id_out)
br.sendTransform((p[0], p[1], p[2]),
wqb,
odom_msg.header.stamp,
self.body_frame_id,
self.world_frame_id)
br.sendTransform(((p[0], p[1], p[2])),
wqc,
odom_msg.header.stamp,
self.intermediate_frame_id,
self.world_frame_id)
#### path ####
pose = PoseStamped()
pose.header = odom_msg.header
pose.pose.position.x = p[0]
pose.pose.position.y = p[1]
pose.pose.position.z = p[2]
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.path.append(pose)
def print_data(userdata):
res = userdata.ttop_data[0]
print res
if not recognize_objects:
pub = res.pose # If the recognize_objects parameter is False
z = res.pose.pose.position.z # Height of the table
minx = res.x_min
miny = res.y_min
maxx = res.x_max
maxy = res.y_max
header = res.pose.header
else:
pub = res.table.pose # If the recognize_objects parameter is True
z = res.table.pose.pose.position.z # Height of the table
minx = res.x_min
miny = res.y_min
maxx = res.x_max
maxy = res.y_max
header = res.table.pose.header
pub.pose = transform_pose(pub.pose, pub.header.frame_id, 'base_link', timeout=3)
#Test with min and max values
ps = PoseStamped()
ps.header = header
#Select one to publish:
#ps.pose = Pose(Point(minx, miny, z), Quaternion()) # Down right (from the back view of the robot)
#ps.pose = Pose(Point(minx, maxy, z), Quaternion()) # Down left (from the back view of the robot)
#ps.pose = Pose(Point(maxx, miny, z), Quaternion()) # Up right (from the back view of the robot)
#ps.pose = Pose(Point(maxx, maxy, z), Quaternion()) # Up left (from the back view of the robot)
#ps.pose = Pose(Point((minx+maxx)/2, (miny+maxy)/2, z), Quaternion()) # Center of the table
ps.pose = Pose(Point(minx-DIST_TO_TABLE, (miny+maxy)/2, z), Quaternion()) # Pos for the navigation goal
raw_input('Press a key when ready to publish the data.\n') # To wait for publish
publisher.publish(ps)
return succeeded
def callback(data):
p = PoseStamped()
p.header = data.header
p.pose.position = data.transform.translation
p.pose.orientation = data.transform.rotation
pub.publish(p);
def otld_callback(self, data):
print "Heard: " + str(data)
# get the depth image
s = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.p2callback)
while not self.got_pointcloud:
# do nothing
print "Waiting for pointcloud"
rospy.sleep(0.1)
s.unregister()
self.got_pointcloud = False
# get the depth of the tracked area
# Header header
# int32 x
# int32 y
# int32 width
# int32 height
# float32 confidence
# probably first get the centroid, we should do a median here or something
centerx = data.x + data.width / 2
centery = data.y + data.height / 2
#print "self.pointcloud.row_step: " + str(self.pointcloud.row_step) + " self.pointcloud.point_step: " + str(self.pointcloud.point_step)
print "centerx: " + str(centerx) + " centery: " + str(centery)
#pointdepth = self.pointcloud.data[self.pointcloud.row_step * centery : self.pointcloud.row_step * centery + self.pointcloud.point_step]
#point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[centerx, centery])
point = read_points(self.pointcloud, field_names=None, skip_nans=False, uvs=[[centerx, centery]])
print "point is: " + str(point)
for item in point:
print "item is: " + str(item)
print "x: "+ str(item[0]) + " y: " + str(item[1]) + ", z: " + str(item[2])
# create PoseStamped
pose = PoseStamped()
pose.header = std_msgs.msg.Header()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "camera_link"
pose.pose = Pose()
pose.pose.position.x = item[2] # kinect Z value, [2], is X in TF of camera_link
pose.pose.position.y = - item[0] # kinect X value, [0], is -Y in TF of camera_link
pose.pose.position.z = - item[1] # kinect Y value, [1], is -Z in TF of camera_link
pose.pose.orientation.w = 1
# send PoseStamped
self.pub.publish(pose)
arrow = Marker()
arrow.header.frame_id = "camera_link"
arrow.header.stamp = rospy.Time.now()
arrow.type = Marker.ARROW
arrow.points = [Point(0,0,0), Point(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z)]
#arrow.points = [Point(0,0,0), Point(0,0,1)]
arrow.scale.x = 0.1 # anchura del palo
arrow.scale.y = 0.15 # anchura de la punta
# arrow.scale.z = 0.1
arrow.color.a = 1.0
arrow.color.r = 1.0
arrow.color.g = 1.0
arrow.color.b = 1.0
self.marker_publisher.publish(arrow)
def get_my_posestamped(self):
self.tf_listener.waitForTransform('map', 'odom', rospy.Time(0), rospy.Duration(10.0))
ps = PoseStamped()
ps.pose = self.odom.pose.pose
ps.header = self.odom.header
current_pose_stamped_in_map = self.tf_listener.transformPose('/map', fuck_the_time(ps))
return current_pose_stamped_in_map
def publisher(paths):
publisher = rospy.Publisher('pathfinder', Path, queue_size=20)
rospy.init_node('path_publisher', anonymous=True)
rate = rospy.Rate(10) # 10 hertz
msgs=[] #list for all the messages we need to publish
for path in paths:
msg = Path() #new path message
msg.header = create_std_h() #add a header to the path message
for x,y,z in path:
#create a pose for each position in the path
#position is the only field we care about, leave the rest as 0
ps = PoseStamped()
ps.header=create_std_h()
i=Pose()
i.position=Point(x,y,z)
ps.pose=i
msg.poses.append(ps)
msgs.append(msg)
while not rospy.is_shutdown(): # while we're going
#publish each message
for m in msgs:
#rospy.loginfo(m)
publisher.publish(m)
rate.sleep()
def publishGoal(self, state):
# Desired states.
x_des, v_des, R_des, w_des = self.getGoalState(state)
q_des = Geometry.Quaternion()
q_des.setFromRotationMatrix(R_des)
pose_msg = PoseStamped()
pose_msg.header = state.header
pose_msg.pose.position.x = x_des[0]
pose_msg.pose.position.y = x_des[1]
pose_msg.pose.position.z = x_des[2]
pose_msg.pose.orientation.x = q_des.x
pose_msg.pose.orientation.y = q_des.y
pose_msg.pose.orientation.z = q_des.z
pose_msg.pose.orientation.w = q_des.w
self.goal_pub.publish(pose_msg)
if pose_msg.header.seq % self.goal_path_pub_factor == 0:
self.goal_path_pub.publish(self.path)
return
def pose_Pub():
# init a node
rospy.init_node('pose_pub', log_level=rospy.INFO, anonymous=True)
# init publisher
pub = rospy.Publisher('pose', PoseStamped, queue_size=10)
# 20 hz
rate = rospy.Rate(1)
poseStamped = PoseStamped()
poseStamped.header = Header()
poseStamped.header.frame_id = '0'
poseStamped.pose = Pose()
poseStamped.pose.position = Point()
poseStamped.pose.position.x = 0
poseStamped.pose.position.y = 0
poseStamped.pose.position.z = 0
poseStamped.pose.orientation = Quaternion()
poseStamped.pose.orientation.x = 0
poseStamped.pose.orientation.y = 0
poseStamped.pose.orientation.z = 0
poseStamped.pose.orientation.w = 0
while not rospy.is_shutdown():
try:
# send
pub.publish(poseStamped)
rospy.loginfo('send msg to the topic: %s', 'pose')
rate.sleep()
poseStamped.pose.position.x = poseStamped.pose.position.x + 1
poseStamped.pose.position.y = poseStamped.pose.position.y + 1
except rospy.ROSException as e:
rospy.logerr('%s', e)
def faces_callback(self, markers):
n = len(markers.markers)
pub = MarkerArray()
for i in xrange(0, n):
#print "yay"
tmp = markers.markers[i]
#print tmp
tmpPose = PoseStamped()
tmpPose.header = tmp.header
tmpPose.pose = tmp.pose
try:
self.listener.waitForTransform(tmp.header.frame_id, '/map', tmp.header.stamp, rospy.Duration(4.5))
ret = self.listener.transformPose('/map', tmpPose)
except tf.LookupException:
print "lookup"
continue
except tf.ConnectivityException:
print "conn"
continue
except tf.ExtrapolationException:
print "extrapolation"
continue
tmp.pose = ret.pose
tmp.header = ret.header
pub.markers.append(tmp)
self.markers_pub.publish(pub)
def get_a_list_of_pose_to_goal(self, start, goal, tf_listener, dist_limit=None, pathpub=None, wppub=None, skip=1):
resolution = self.og.info.resolution
width = self.og.info.width
height = self.og.info.height
offsetX = self.og.info.origin.position.x
offsetY = self.og.info.origin.position.y
tf_listener.waitForTransform('odom', 'map', rospy.Time(0), rospy.Duration(10.0))
tstart = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(start))
tgoal = tf_listener.transformPose(self.og.header.frame_id, fuck_the_time(goal))
if dist_limit is not None:
tgoal = self.limit_max_dist(tstart, tgoal, dist_limit)
if wppub is not None:
ttgoal = tf_listener.transformPose('map', fuck_the_time(tgoal))
wppub.publish(ttgoal)
start_grid_pos = pose2gridpos(tstart.pose, resolution, offsetX, offsetY)
goal_grid_pos = pose2gridpos(tgoal.pose, resolution, offsetX, offsetY)
try:
path = aStar(self.make_navigable_gridpos(), start_grid_pos, goal_grid_pos, self.astarnodescb)
wp = self.getWaypoints(path, tgoal, publisher=pathpub)
list_of_pose = []
# magic number. skip a few waypoints in the beginning
for stuff in wp:
retp = PoseStamped()
retp.pose = stuff.pose
retp.header = self.og.header
list_of_pose.append(retp)
return list_of_pose
except NoPathFoundException as e:
raise e
def ContourinfoLists_callback(self, contourinfolists):
if self.initialized:
nContours = len(contourinfolists.x)
if (nContours==1):
self.textError = ''
poseContour = PoseStamped(header=contourinfolists.header,
pose=Pose(position=Point(x=contourinfolists.x[0],
y=contourinfolists.y[0]))
)
poseVisual = PoseStamped()
poseVisual.header = contourinfolists.header
poseVisual.header.frame_id = 'Arena'
poseVisual.pose = self.PoseArenaFromImage(poseContour) # Gives the position of the contour in millimeters.
# Point to the cache non-working entry.
iLoading = (self.iWorking+1) % 2
self.cachePose[iLoading] = poseVisual
self.areaVisual = contourinfolists.area[0]
self.eccVisual = contourinfolists.ecc[0]
self.xMin = min(self.xMin, poseVisual.pose.position.x)
self.xMax = max(self.xMax, poseVisual.pose.position.x)
self.yMin = min(self.yMin, poseVisual.pose.position.y)
self.yMax = max(self.yMax, poseVisual.pose.position.y)
elif (nContours==0):
#rospy.logwarn('ERROR: No objects detected.')
self.textError = 'ERROR: No objects detected.'
elif (nContours>1):
#rospy.logwarn('ERROR: More than one object detected.')
self.textError = 'ERROR: More than one object detected.'
def MovetoScanRange():
#Combine position and orientation in a PoseStamped() message
move_to_pose = PoseStamped()
move_to_pose.header=Header(stamp=rospy.Time.now(), frame_id='base')
move_to_pose.pose.position=Point(
x=-0.25, #-0.25
y=0.7, #0.7
z=0.3, #0.3
)
move_to_pose.pose.orientation=Quaternion(
x=-0.5,
y=0.5,
z=0.5,
w=0.5,
)
#Send PoseStamped() message to Baxter's movement function
pub_baxtermovement.publish(move_to_pose)
rospy.sleep(2)
global first_flag
first_flag = True
return
def call_grasp(obj,world): #TODO #adding grasping
tosay = "I'm going to grasp the " + obj
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_grasp '+obj)
rospy.logwarn(world.item.object_pose)
#############################################################################
if SKILLS :
pose_object_to_grasp = PoseStamped()
pose_object_to_grasp.header = world.item.object_pose.header
pose_object_to_grasp.pose = world.item.object_pose.pose.pose
if (time.time()-TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while(out=='aborted' and tries<3):
sm = pick_object_sm(pose_object_to_grasp) #if not workng, blame chang
out = sm.execute()
tries = tries+1
#grasping here
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
def ComparePresentIDtoScannedID(currentstocking):
if scanned_ar == tag_id:
#Store pose of QR code from camera into local variables
#PoseStamp messages contains a header and a pose. We care only about the pose so we extract it.
position_visp = tag_msg.pose.position
quat_visp = tag_msg.pose.orientation
#rospy.loginfo("Tag Point Position: [ %f, %f, %f ]"%(position_visp.x, position_visp.y, position_visp.z))
#rospy.loginfo("Tag Quat Orientation: [ %f, %f, %f, %f]"%(quat_visp.x, quat_visp.y, quat_visp.z, quat_visp.w))
tag_pos_x = position_visp.x
tag_pos_y = position_visp.y
tag_pos_z = position_visp.z
tag_quat_x = quat_visp.x
tag_quat_y = quat_visp.y
tag_quat_z = quat_visp.z
tag_quat_w = quat_visp.w
move_to_pose = PoseStamped()
move_to_pose.header=Header(stamp=rospy.Time.now(), frame_id='base')
move_to_pose.pose.position=Point(
x=tag_pos_x,
y=tag_pos_y, #we move to the ar code on the next stocking
z=tag_pos_z,
)
move_to_pose.pose.orientation=Quaternion(
x=tag_quat_x,
y=tag_quat_y,
z=tag_quat_z,
w=tag_quat_w,
)
def pose_callback(data):
# extract x and y position information
x = data.pose.pose.position.x
y = data.pose.pose.position.y
# package and broadcast
p = Point()
p.x = x
p.y = y
# send it
pub_beacon.publish(p)
# and broadbast all odoms on a common channel
pub_odom_all.publish(data)
# publish pose info for viewing
pose_out = PoseStamped()
pose_out.header = data.header
pose_out.header.frame_id = '/world'
pose_out.pose = data.pose.pose
pub_pose.publish(pose_out)
# and twist stamped for DMS interface
vel_out = TwistStamped()
vel_out.header = data.header
vel_out.header.frame_id = '/world'
vel_out.twist = data.twist.twist
pub_vel.publish(vel_out)
# transform
tf_broadcast.sendTransform((x, y, 0.0),
(data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w),
rospy.Time.now(),frame_name,"/world")
def MovetoScanStockingRange():
#Send Baxter back to starting point to find next stocking
move_to_pose = PoseStamped()
move_to_pose.header=Header(stamp=rospy.Time.now(), frame_id='base')
move_to_pose.pose.position=Point(
x=-0.25,
y=0.7,
z=0.3,
)
move_to_pose.pose.orientation=Quaternion(
x=-0.5,
y=0.5,
z=0.5,
w=0.5,
)
#Send PoseStamped() message to Baxter's movement function
pub_baxtermovement.publish(move_to_pose)
rospy.sleep(10)
global first_flag
first_flag = True
return
def test_attctl(self):
# set some attitude and thrust
att = PoseStamped()
att.header = Header()
att.header.frame_id = "base_footprint"
att.header.stamp = rospy.Time.now()
quaternion = quaternion_from_euler(0.15, 0.15, 0)
att.pose.orientation = Quaternion(*quaternion)
throttle = Float64()
throttle.data = 0.6
# does it cross expected boundaries in X seconds?
count = 0
timeout = 120
while count < timeout:
# update timestamp for each published SP
att.header.stamp = rospy.Time.now()
self.pub_att.publish(att)
self.pub_thr.publish(throttle)
self.rate.sleep()
if (self.local_position.pose.position.x > 5
and self.local_position.pose.position.z > 5
and self.local_position.pose.position.y < -5):
break
count = count + 1
self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set")
self.assertTrue(self.control_mode.flag_control_attitude_enabled, "flag_control_attitude_enabled is not set")
self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set")
self.assertTrue(count < timeout, "took too long to cross boundaries")
def visualize(self):
'''
Publish various visualization messages.
'''
if not self.DO_VIZ:
return
if self.pose_pub.get_num_connections() > 0 and isinstance(self.inferred_pose, np.ndarray):
# Publish the inferred pose for visualization
ps = PoseStamped()
ps.header = Utils.make_header("map")
ps.pose.position.x = self.inferred_pose[0]
ps.pose.position.y = self.inferred_pose[1]
ps.pose.orientation = Utils.angle_to_quaternion(self.inferred_pose[2])
self.pose_pub.publish(ps)
if self.particle_pub.get_num_connections() > 0:
# publish a downsampled version of the particle distribution to avoid a lot of latency
if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES:
# randomly downsample particles
proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights)
# proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES)
self.publish_particles(self.particles[proposal_indices,:])
else:
self.publish_particles(self.particles)
if self.pub_fake_scan.get_num_connections() > 0 and isinstance(self.ranges, np.ndarray):
# generate the scan from the point of view of the inferred position for visualization
self.viz_queries[:,0] = self.inferred_pose[0]
self.viz_queries[:,1] = self.inferred_pose[1]
self.viz_queries[:,2] = self.downsampled_angles + self.inferred_pose[2]
self.range_method.calc_range_many(self.viz_queries, self.viz_ranges)
self.publish_scan(self.downsampled_angles, self.viz_ranges)
def handle_xy_goal(req):
p = PoseStamped()
p.header = req.header
p.pose = req.pose.pose
rospy.loginfo("GPS goal converted to xy: " + str(p.pose.position.x) + ", " + str(p.pose.position.y) + ". Publishing..")
global pub_goal
pub_goal.publish(p)
def combinePoses(ps0, ps1, op=operator.add, listener=None):
"""
Returns a PoseStamped of op(ps0,ps1)
"""
# must be in same reference frame
if listener != None:
try:
ps0, ps1 = convertToSameFrameAndTime(ps0, ps1, listener)
except tf.Exception:
return PoseStamped()
if ps0 == None or ps1 == None:
return False
xtrans0, ytrans0, ztrans0 = ps0.pose.position.x, ps0.pose.position.y, ps0.pose.position.z
xtrans1, ytrans1, ztrans1 = ps1.pose.position.x, ps1.pose.position.y, ps1.pose.position.z
wrot0, xrot0, yrot0, zrot0 = ps0.pose.orientation.w, ps0.pose.orientation.x, ps0.pose.orientation.y, ps0.pose.orientation.z
wrot1, xrot1, yrot1, zrot1 = ps1.pose.orientation.w, ps1.pose.orientation.x, ps1.pose.orientation.y, ps1.pose.orientation.z
ps0rot0, ps0rot1, ps0rot2 = tft.euler_from_quaternion([xrot0, yrot0, zrot0, wrot0])
ps1rot0, ps1rot1, ps1rot2 = tft.euler_from_quaternion([xrot1, yrot1, zrot1, wrot1])
addedPoint = Point(op(xtrans0,xtrans1), op(ytrans0,ytrans1), op(ztrans0,ztrans1))
addedEuler = [op(ps0rot0,ps1rot0), op(ps0rot1,ps1rot1), op(ps0rot2,ps1rot2)]
addedQuaternion = tft.quaternion_from_euler(addedEuler[0], addedEuler[1], addedEuler[2])
addedOrientation = Quaternion(addedQuaternion[0], addedQuaternion[1], addedQuaternion[2], addedQuaternion[3])
addedPose = PoseStamped()
addedPose.header = ps0.header
addedPose.pose.position = addedPoint
addedPose.pose.orientation = addedOrientation
return addedPose
def map_and_detect( self ):
print "Get object pose"
rospy.sleep(ROBUST_POSE_TIMEOUT)
self.g_object_pose = False
counter=0
while self.g_object_pose == False and counter < DETECTION_TIMEOUT:
self.g_object_pose = self.get_object_pose()
counter = counter+1
if self.g_object_pose == False:
self.state = 12
return False
object_mesh_pose = PoseStamped()
object_mesh_pose.header = Header(stamp = rospy.Time.now(), frame_id = '/world')
object_mesh_pose.pose = self.g_object_pose
object_mesh_pose.pose.orientation = rotate_orientation_quaternion(object_mesh_pose.pose.orientation)
print "Adding the object mesh to the scene"
rospy.wait_for_service('place_object')
req = place_objectRequest();
req.object_pose = object_mesh_pose;
req.object_name = self.g_decision_making_state.object_name;
print self.g_decision_making_state.object_name;
try:
place_object_caller = rospy.ServiceProxy('place_object', place_object)
resp1 = place_object_caller(req);
except rospy.ServiceException, e:
print "Service call failed:"
def reach_position(self, x, y, z, timeout):
# set a position setpoint
pos = PoseStamped()
pos.header = Header()
pos.header.frame_id = "base_footprint"
pos.pose.position.x = x
pos.pose.position.y = y
pos.pose.position.z = z
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = 0 # North
yaw = math.radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
pos.pose.orientation = Quaternion(*quaternion)
# does it reach the position in X seconds?
count = 0
while count < timeout:
# update timestamp for each published SP
pos.header.stamp = rospy.Time.now()
self.pub_spt.publish(pos)
self.helper.bag_write('mavros/setpoint_position/local', pos)
if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5):
break
count = count + 1
self.rate.sleep()
self.assertTrue(count < timeout, "took too long to get to position")
def publishWayPoints(xPos, yPos, angle, w_p):
print "publishing waypoint"
pub = rospy.Publisher("/move_base_simple/goal", PoseStamped)
goal = PoseStamped()
initTime = rospy.get_time()
mapWidth = globalCostMapGrid.info.width
mapOriginX = int(math.floor(globalCostMapGrid.info.origin.position.x * 20))
mapOriginY = int(math.floor(globalCostMapGrid.info.origin.position.y * 20))
goal.header = g.header
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = xPos
goal.pose.position.y = yPos
goal.pose.position.z = 0
quat = quaternion_from_euler(0, 0, angle)
goal.pose.orientation.x = quat[0]
goal.pose.orientation.y = quat[1]
goal.pose.orientation.z = quat[2]
goal.pose.orientation.w = quat[3]
valX = int(math.floor(d.way_points[w_p][0] * 20)) - mapOriginX
valY = int(math.floor(d.way_points[w_p][1] * 20)) - mapOriginY
r = rospy.Rate(0.7)
while math.sqrt(math.pow(g.xPos - d.way_points[w_p][0], 2) + math.pow(g.yPos - d.way_points[w_p][1], 2)) > 0.5:
print "d:", math.sqrt(math.pow(g.xPos - d.way_points[w_p][0], 2) + math.pow(g.yPos - d.way_points[w_p][1], 2))
finalTime = rospy.get_time()
changeTime = finalTime - initTime
if (globalCostMapGrid.data[(valX * mapWidth) + valY] > g.threshold) or (changeTime > 120):
break
else:
pub.publish(goal)
def callback(data):
p = PoseStamped()
p.header = data.header
p.header.frame_id = 'world'
p.pose = data.pose.pose
# Publish PoseStamped
pub.publish(p);
def draw_markers(markers):
# rospy.loginfo('Drawing markers ' + str(list(markers.ids)).\
# replace('[', '{').replace(']', '}'))
for id, pose in zip(markers.ids, markers.poses):
ps = PoseStamped()
ps.header = markers.header
ps.pose = pose
draw_axes(pub, id, 'ar_markers', ps, text=str(id))
def handle_xy_goal(req):
new_pos = PoseStamped()
new_pos.header = req.header
new_pos.pose = req.pose.pose
rospy.loginfo("GPS goal converted to xy: " + str(new_pos.pose.position.x) + ", " + str(new_pos.pose.position.y) + ". Publishing..")
global pub_goal
pub_goal.publish(new_pos)
global p
p.append(new_pos)
def to_pose_stamped(self):
ps = PoseStamped()
ps.header = self._to_header()
ps.pose = self.to_pose()
return ps
def createPath(self):
# Creating a linear path which goes from (1,3) to (8,3)
# Theta equals zero which is in quaternion (1, 0, 0, 0)
dx = 7 / 300.0
path_local = [(1, 3)]
curr_pos = PoseStamped()
curr_pos.pose.position.x = 1
curr_pos.pose.position.y = 3
curr_pos.pose.position.z = 0
curr_pos.pose.orientation.x = 1
curr_pos.pose.orientation.y = 0
curr_pos.pose.orientation.z = 0
curr_pos.pose.orientation.w = 0
curr_pos.header.seq = self.path.header.seq + 1
self.path.header.frame_id = "world" #---------------->header je world
self.path.header.stamp = rospy.Time.now()
curr_pos.header = self.path.header
curr_pos.header.frame_id = "world"
self.path.poses.append(curr_pos)
for i in range(1, 121):
curr_pos = PoseStamped()
x, y = path_local[i - 1]
path_local.append((x + dx, y))
curr_pos.pose.position.x = x + dx
curr_pos.pose.position.y = y
curr_pos.pose.position.z = 0
curr_pos.pose.orientation.x = 1
curr_pos.pose.orientation.y = 0
curr_pos.pose.orientation.z = 0
curr_pos.pose.orientation.w = 0
curr_pos.header.seq = self.path.header.seq + 1
self.path.header.stamp = rospy.Time.now()
curr_pos.header.stamp = self.path.header.stamp
self.path.poses.append(curr_pos)
for i in range(121, 181):
curr_pos = PoseStamped()
x, y = path_local[i - 1]
path_local.append((x + dx, y + dx))
curr_pos.pose.position.x = x + dx
curr_pos.pose.position.y = y + dx
curr_pos.pose.position.z = 0
# angle of 45 degrees
curr_pos.pose.orientation.x = 0.9238795
curr_pos.pose.orientation.y = 0.38268343
curr_pos.pose.orientation.z = 0
curr_pos.pose.orientation.w = 0
curr_pos.header.seq = self.path.header.seq + 1
self.path.header.stamp = rospy.Time.now()
curr_pos.header.stamp = self.path.header.stamp
self.path.poses.append(curr_pos)
for i in range(181, 330):
curr_pos = PoseStamped()
x, y = path_local[i - 1]
path_local.append((x + dx, y))
curr_pos.pose.position.x = x + dx
curr_pos.pose.position.y = y
curr_pos.pose.position.z = 0
curr_pos.pose.orientation.x = 1
curr_pos.pose.orientation.y = 0
curr_pos.pose.orientation.z = 0
curr_pos.pose.orientation.w = 0
curr_pos.header.seq = self.path.header.seq + 1
self.path.header.stamp = rospy.Time.now()
curr_pos.header.stamp = self.path.header.stamp
self.path.poses.append(curr_pos)
r = rospy.Rate(50)
while not rospy.is_shutdown():
print "publishamo", 'dx', dx
self.pathPub.publish(self.path)
r.sleep()
def send_ptam(self, topic):
ptam_pos = PoseStamped()
ptam_pos.header = topic.header
ptam_pos.pose = topic.pose.pose
pub_ptam.publish(ptam_pos)
self.setpoint = msg
if __name__ == '__main__':
# rosnode
rospy.init_node('rpy_setpoint_bridge')
pose_pub = rospy.Publisher('body_position_setpoint', PoseStamped, queue_size=1)
pose_msg = PoseStamped()
listener = Listener()
T =1./50
ratio = 1./5
while not rospy.is_shutdown():
if listener.setpoint_received:
# copy header
pose_msg.header = listener.setpoint.header
# just copy translation part
pose_msg.pose.position = listener.setpoint.twist.linear
# change rotation to quaternion
q = quaternion_from_euler(listener.setpoint.twist.angular.x, listener.setpoint.twist.angular.y, listener.setpoint.twist.angular.z)
pose_msg.pose.orientation.x = q[0]
pose_msg.pose.orientation.y = q[1]
pose_msg.pose.orientation.z = q[2]
pose_msg.pose.orientation.w = q[3]
pose_pub.publish(pose_msg)
rospy.sleep(T)
def __call__(self, req):
self.start = Node(req.start.x, req.start.y, req.start.z)
self.goal = Node(req.goal.x, req.goal.y, req.goal.z)
self.tree = [self.start]
response = PlannerResponse()
rospy.loginfo("Planner called from [%f %f %f] to [%f %f %f]" %
(req.start.x, req.start.z, req.start.z, req.goal.x,
req.goal.y, req.goal.z))
n_iters = 10000
sample_area = [[-5 + self.start.x, 5 + self.goal.x],
[-5 + self.start.y, 5 + self.goal.y],
[-2 + self.start.z, 2 + self.goal.z]]
#Run till tne number of iterations are not reached
time1 = time.time()
while n_iters >= 0:
# genertae goal node, biased to return goal as the node with a probability alpha
node = generate_node(sample_area,
self.alpha,
self.goal,
dim=self.dim)
#look for the nearest node in the tree
nearest_node = nn(node, self.tree)
# If distacne to the the nearest node is greater than the threshold
if distance(node, nearest_node, dim=self.dim) > self.delta:
# Make a new node and then check for collision
node = lineTo(node, nearest_node, self.delta, dim=self.dim)
# if no collision , then add it to the tree
if not collision(node, nearest_node):
#print("added")
node.parent = nearest_node
self.tree.append(node)
# Else,
else:
# Check for collision, If no collision add it to the tree
if not collision(node, nearest_node):
#print("added")
node.parent = nearest_node
self.tree.append(node)
# If you approach the goal node, break the loop
if abs(node.x - self.goal.x) < 0.01 and abs(
node.y - self.goal.y) < 0.01 and abs(node.z -
self.goal.z) < 0.01:
#print("goal reached")
response.reached = True
break
if distance(node, self.start, dim=self.dim) > 3.0:
response.reached = False
break
if self.animation:
self.animate()
# Count iteration as done
n_iters -= 1
#print([(node.x, node.y, node.z) for node in self.tree])
path = self.get_path()
path = path[::-1]
#optim_path = self.optimised_path(path)
path_msg = Path()
path_msg.header = Header(frame_id="world", stamp=rospy.Time.now())
#angle = np.arctan2(self.goal.y - self.start.y, self.goal.x - self.start.x)
for point in path:
pose = PoseStamped()
pose.header = Header(frame_id="world", stamp=rospy.Time.now())
pose.pose.position = Point(point.x, point.y, point.z)
'''if angle < 0:
quat = qfe(0, 0, 180)
print(quat)
pose.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
else:
pose.pose.orientation = Quaternion(0, 0, 0, 1)'''
pose.pose.orientation = Quaternion(0, 0, 0, 1)
path_msg.poses.append(pose)
optim_req = PathOptimiserRequest()
optim_req.crude_path = path_msg
optim_resp = self.optimiser(optim_req)
response.path = optim_resp.optimised_path
"""optim_path_msg = Path()
optim_path_msg.header = Header(frame_id="world", stamp=rospy.Time.now())
for point in optim_path:
pose = PoseStamped()
pose.header = Header(frame_id="world", stamp=rospy.Time.now())
pose.pose.position = Point(point.x, point.y, point.z)
pose.pose.orientation = Quaternion(0, 0, 0, 1)
path_msg.poses.append(pose)
"""
self.path_pub.publish(path_msg)
self.optim_path_pub.publish(optim_resp.optimised_path)
return response #, optim_path[::-1]
def ctrl_callback(self, ctrl_flag_msg):
print('----ctrl callback----')
start_time = time.time()
pose = self.pose.copy()
pose[2] = self.normalize(pose[2])
self.old_obsv = np.array(self.curr_obsv, copy=True)
self.curr_obsv = np.array(self.obsv, copy=True)
update_flag =np.array_equal(self.old_obsv, self.curr_obsv)
# for i in range(20):
# print('update_flag: ', update_flag)
########
# landmarks table test
########
self.obsv_table = []
tid = 0
for olm in self.curr_obsv:
# for each observation
dist_list = []
for raw_lm in self.real_landmarks:
dist = np.sqrt( (raw_lm[0]-olm[0])**2 + (raw_lm[1]-olm[1])**2 )
dist_list.append(dist)
oid = np.argmin(dist_list)
self.obsv_table.append(oid)
if oid in self.lm_id:
pass
else:
self.lm_id.append(oid)
self.ekf_mean = np.concatenate((self.ekf_mean, olm))
self.ekf_cov = np.block([[self.ekf_cov, np.zeros((self.ekf_cov.shape[0],2))],
[np.zeros((2,self.ekf_cov.shape[0])), np.eye(2)*1000]])
num_lm = int(0.5 * (self.ekf_mean.shape[0] - 3))
cube_list = Marker()
cube_list.header.frame_id = 'odom'
cube_list.header.stamp = rospy.Time.now()
cube_list.ns = 'landmark_map'
cube_list.action = Marker.ADD
cube_list.pose.orientation.w = 1.0
cube_list.id = 0
cube_list.type = Marker.CUBE_LIST
cube_list.scale.x = 0.05
cube_list.scale.y = 0.05
cube_list.scale.z = 0.5
cube_list.color.r = 1.0
cube_list.color.g = 1.0
cube_list.color.a = 1.0
for i in range(num_lm):
landmark = self.ekf_mean[3+i*2:5+i*2]
p = Point()
p.x = landmark[0]
p.y = landmark[1]
p.z = 0.25
cube_list.points.append(p)
self.map_pub.publish(cube_list)
########
# ekf
########
odom_diff = pose - self.raw_odom_traj[-1]
self.raw_odom_traj.append(pose.copy())
prev_ctrl = np.array([0., 0.])
if len(self.log['ctrls']) > 0:
prev_ctrl = self.log['ctrls'][-1].copy()
G = np.eye(self.ekf_mean.shape[0])
G[0][2] = -np.sin(self.ekf_mean[2]) * prev_ctrl[0] * 0.1
G[1][2] = np.cos(self.ekf_mean[2]) * prev_ctrl[1] * 0.1
num_lm = int(0.5 * (self.ekf_mean.shape[0]-3))
BigR = np.block([
[self.ekf_R, np.zeros((3, 2*num_lm))],
[np.zeros((2*num_lm, 3)), np.zeros((2*num_lm, 2*num_lm))]
])
self.ekf_cov = G @ self.ekf_cov @ G.T + BigR
# self.ekf_mean[0] = pose[0]
# self.ekf_mean[1] = pose[1]
# self.ekf_mean[2] = pose[2]
# self.ekf_mean[2] = self.normalize(self.ekf_mean[2])
# self.ekf_mean[0] += np.cos(self.ekf_mean[2]) * prev_ctrl[0] * 0.1
# self.ekf_mean[1] += np.sin(self.ekf_mean[2]) * prev_ctrl[0] * 0.1
# self.ekf_mean[2] += prev_ctrl[1] * 0.1
# self.ekf_mean[2] = self.normalize(self.ekf_mean[2])
self.ekf_mean[0] += odom_diff[0]
self.ekf_mean[1] += odom_diff[1]
self.ekf_mean[2] += odom_diff[2]
self.ekf_mean[2] = self.normalize(self.ekf_mean[2])
if update_flag is False:
# if True:
print('update_flag is False')
num_obsv = len(self.curr_obsv)
H = np.zeros((2*num_obsv, self.ekf_mean.shape[0]))
r = self.ekf_mean[0:3].copy()
ref_obsv = []
print('num_obsv: ', num_obsv)
for i in range(num_obsv):
idx = i*2
# lid = self.obsv_table[i]
lid = np.where(self.lm_id==self.obsv_table[i])[0][0]
lm = self.ekf_mean[3+lid*2 : 5+lid*2]
zr = np.sqrt((r[0]-lm[0])**2 + (r[1]-lm[1])**2)
H[idx][0] = (r[0]-lm[0]) / zr
H[idx][1] = (r[1]-lm[1]) / zr
H[idx][2] = 0
H[idx][3+2*lid] = -(r[0]-lm[0]) / zr
H[idx][4+2*lid] = -(r[1]-lm[1]) / zr
H[idx+1][0] = -(r[1]-lm[1]) / zr**2
H[idx+1][1] = (r[0]-lm[0]) / zr**2
H[idx+1][2] = -1
H[idx+1][3+2*lid] = (r[1]-lm[1]) / zr**2
H[idx+1][4+2*lid] = -(r[0]-lm[0]) / zr**2
ref_obsv.append(self.range_bearing(r, lm))
ref_obsv = np.array(ref_obsv)
BigQ = linalg.block_diag(*[self.ekf_Q for _ in range(num_obsv)])
mat1 = np.dot(self.ekf_cov, H.T)
mat2 = np.dot(np.dot(H, self.ekf_cov), H.T)
mat3 = np.linalg.inv(mat2 + BigQ)
K = np.dot(mat1, mat3)
'''
ori_obsv = []
for obsv in self.curr_obsv:
temp = self.range_bearing(r, obsv)
temp[1] = self.normalize(temp[1])
ori_obsv.append(temp.copy())
ori_obsv = np.array(ori_obsv)
'''
ori_obsv = np.array(self.raw_scan)
if len(ori_obsv) == 0:
pass
else:
# raw_scan[:,1] = self.normalize(raw_scan[:,1])
print('ori_obsv.shape: ', ori_obsv.shape)
delta_z = ori_obsv - ref_obsv
delta_z[:,1] = self.normalize(delta_z[:,1])
delta_z = delta_z.reshape(-1)
self.ekf_mean += K @ delta_z
self.ekf_cov -= K @ H @ self.ekf_cov
print('r: {} | {}'.format(pose, self.ekf_mean[0:3]))
print(len(self.ekf_mean))
ekf_odom = Odometry()
ekf_odom.header = copy(self.odom_header)
ekf_odom.child_frame_id = "base_footprint"
ekf_odom.pose.pose.position.x = self.ekf_mean[0]
ekf_odom.pose.pose.position.y = self.ekf_mean[1]
ekf_odom.pose.covariance[0] = self.ekf_cov[0][0]
ekf_odom.pose.covariance[1] = self.ekf_cov[1][1]
self.ekf_pub.publish(ekf_odom)
print('ekf cov: ', np.trace(self.ekf_cov))
########
# ctrl
########
idx = self.log['count'] % 10
obstacles = []
for lm in self.ekf_mean[3:].reshape(-1, 2).copy():
obstacles.append(lm)
if len(self.raw_scan)>0:
raw_scan = self.raw_scan[-1]
raw_lm_x = self.ekf_mean[0] + np.cos(raw_scan[1] + self.ekf_mean[2]) * raw_scan[0]
raw_lm_y = self.ekf_mean[0] + np.sin(raw_scan[1] + self.ekf_mean[2]) * raw_scan[0]
obstacles.append(np.array([raw_lm_x, raw_lm_y]))
obstacles = np.array(obstacles)
# self.erg_ctrl.barr.update_obstacles(self.obsv)
# self.erg_ctrl.barr.update_obstacles(self.ekf_mean[3:].reshape(-1, 2).copy())
self.erg_ctrl.barr.update_obstacles(obstacles.copy())
self.t_dist.update_og(self.ekf_mean.copy(), self.ekf_cov.copy())
# _, ctrl_seq = self.erg_ctrl(pose.copy(), seq=True)
_, ctrl_seq = self.erg_ctrl(self.ekf_mean[0:3].copy(), seq=True)
if idx == 0:
print('update ctrl seq')
grid_vals = self.t_dist.update_dist_val(self.ekf_mean.copy(), self.ekf_cov.copy(), self.imcov)
print('grid_vals: ', np.sum(grid_vals))
np.save('grid_vals.npy', grid_vals)
self.phik = convert_phi2phik(self.basis, self.t_dist.grid_vals, self.t_dist.grid, self.size)
self.erg_ctrl.phik = self.phik
print('phik updated')
self.ctrl_seq = ctrl_seq.copy()
ctrl = self.ctrl_seq[idx]
ctrl_lin = ctrl[0]
ctrl_ang = ctrl[1]
vel_msg = Twist()
vel_msg.linear.x = ctrl_lin
vel_msg.linear.y = 0.0
vel_msg.linear.z = 0.0
vel_msg.angular.x = 0.0
vel_msg.angular.y = 0.0
vel_msg.angular.z = ctrl_ang
self.ctrl_pub.publish(vel_msg)
else:
print('follow ctrl seq')
ctrl = self.ctrl_seq[idx]
ctrl_lin = ctrl[0]
ctrl_ang = ctrl[1]
vel_msg = Twist()
vel_msg.linear.x = ctrl_lin
vel_msg.linear.y = 0.0
vel_msg.linear.z = 0.0
vel_msg.angular.x = 0.0
vel_msg.angular.y = 0.0
vel_msg.angular.z = ctrl_ang
self.ctrl_pub.publish(vel_msg)
# log
self.log['count'] += 1
self.log['traj'].append(pose.copy())
self.log['ctrls'].append(ctrl.copy())
# publish predicted trajectory
self.path_msg = Path()
self.path_msg.header = copy(self.odom_header)
dummy_pose = self.ekf_mean[0:3].copy() #pose.copy()
for i in range(idx, 80):
dummy_ctrl = self.ctrl_seq[i]
dummy_pose += 0.1 * np.array([cos(dummy_pose[2])*dummy_ctrl[0],
sin(dummy_pose[2])*dummy_ctrl[0],
dummy_ctrl[1]])
pose_msg = PoseStamped()
pose_msg.header = copy(self.odom_header)
pose_msg.pose.position.x = dummy_pose[0]
pose_msg.pose.position.y = dummy_pose[1]
self.path_msg.poses.append(copy(pose_msg))
self.path_pub.publish(self.path_msg)
print('elasped time: {}'.format(time.time()-start_time))
#
# fk_link_names
fk_link_names = ["racket"]
joint_state = JointState()
joint_state.header = Header()
joint_state.header.frame_id = "racket"
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"
]
joint_state.position = [0, 0, 0, 0, 0, 0]
robot_joint_state = RobotState()
robot_joint_state.joint_state = joint_state
position_1 = PoseStamped()
position_1.header = ref_frame
position_1.pose.position.x = 0.68
position_1.pose.position.y = 0.1
position_1.pose.position.z = 0.5
position_1.pose.orientation.x = 0
position_1.pose.orientation.y = 0
position_1.pose.orientation.z = 0
position_1.pose.orientation.w = 1
position_2 = PoseStamped()
position_2.header = ref_frame
position_2.pose.position.x = 0.7
position_2.pose.position.y = 0
position_2.pose.position.z = 0.6
position_2.pose.orientation.x = 0
position_2.pose.orientation.y = 0
def write_to_pixhawk(data):
ptam_pos = PoseStamped()
ptam_pos.header = data.header
ptam_pos.pose = data.pose.pose
pub_ptam.publish(ptam_pos)
def test_attctl(self):
"""Test offboard attitude control"""
# FIXME: hack to wait for simulation to be ready
while not self.has_global_pos:
self.rate.sleep()
# set some attitude and thrust
att = PoseStamped()
att.header = Header()
att.header.frame_id = "base_footprint"
att.header.stamp = rospy.Time.now()
quaternion = quaternion_from_euler(0.25, 0.25, 0)
att.pose.orientation = Quaternion(*quaternion)
throttle = Float64()
throttle.data = 0.7
armed = False
# does it cross expected boundaries in X seconds?
count = 0
timeout = 120
while count < timeout:
# update timestamp for each published SP
att.header.stamp = rospy.Time.now()
self.pub_att.publish(att)
#self.helper.bag_write('mavros/setpoint_attitude/attitude', att)
self.pub_thr.publish(throttle)
#self.helper.bag_write('mavros/setpoint_attitude/att_throttle', throttle)
self.rate.sleep()
# FIXME: arm and switch to offboard
# (need to wait the first few rounds until PX4 has the offboard stream)
if not armed and count > 5:
self._srv_cmd_long(False, 176, False, 1, 6, 0, 0, 0, 0, 0)
# make sure the first command doesn't get lost
time.sleep(1)
self._srv_cmd_long(
False,
400,
False,
# arm
1,
0,
0,
0,
0,
0,
0)
armed = True
if (self.local_position.pose.position.x > 5
and self.local_position.pose.position.z > 5
and self.local_position.pose.position.y < -5):
break
count = count + 1
self.assertTrue(count < timeout, "took too long to cross boundaries")
def tf_message_to_pose_stamped(tf_msg):
pose_msg = PoseStamped()
pose_msg.header = tf_msg.transforms[0].header
pose_msg.pose.position = tf_msg.transforms[0].transform.translation
pose_msg.pose.orientation = tf_msg.transforms[0].transform.rotation
return pose_msg
def execute_cb(self, goal):
r = rospy.Rate(1)
sucess = True
#self._result.trajectory_start = self.robot.robot.get_current_state()
self.robot.set_start_state_to_current_state()
self._feedback.state = "Open Gripper"
self._as.publish_feedback(self._feedback)
self.robot.robot.get_current_state()
rospy.loginfo('Open Gripper Plan')
plan = self.robot.openGripper()
if plan is None:
rospy.loginfo("Open Gripper plan failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._result.trajectory_descriptions.append('OpenGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Opening gripper"
rospy.loginfo('Opening gripper')
ex_status = self.robot.gripper_execute(plan)
if not ex_status:
rospy.loginfo("Execution to open gripper failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
self._as.publish_feedback(self._feedback)
self.scene.remove_attached_object('gripper_link')
self._feedback.state = "Planning to reach object"
rospy.loginfo('Planning to reach obj')
self._as.publish_feedback(self._feedback)
dimension, target = self.get_target(goal.target_name)
quat = []
if len(goal.possible_grasps) > 0:
quat = [
goal.possible_grasps[0].grasp_pose.pose.orientation.x,
goal.possible_grasps[0].grasp_pose.pose.orientation.y,
goal.possible_grasps[0].grasp_pose.pose.orientation.z,
goal.possible_grasps[0].grasp_pose.pose.orientation.w
]
rospy.loginfo('Pick Quaternion [%s]', quat)
plan = self.robot.ef_pose(target, dimension, orientation=quat)
if plan is None:
rospy.loginfo("Plan to grasp failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._feedback.state = "Going to the object"
rospy.loginfo('Going to Obj')
self._as.publish_feedback(self._feedback)
self._result.trajectory_descriptions.append(
"Going to grasp the object")
self._result.trajectory_stages.append(plan)
ex_status = self.robot.arm_execute(plan)
if not ex_status:
rospy.loginfo("Execution to grasp failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
# self._feedback.state = "Removing obtect to be grasp from the planning scene"
# self._as.publish_feedback(self._feedback)
obj = self.scene.get_objects([goal.target_name])
obj = obj[goal.target_name]
# self.scene.remove_world_object(goal.target_name)#
self._feedback.state = "Attaching object"
self._as.publish_feedback(self._feedback)
pose = PoseStamped()
pose.pose = obj.primitive_poses[0]
pose.header = obj.header
self.scene.attach_box(
"gripper_link", obj.id, pose, obj.primitives[0].dimensions,
['gripper_link', 'gripper_active_link', 'gripper_active2_link'])
rospy.sleep(1)
rospy.sleep(1)
self._feedback.state = "Planning to close the gripper"
self._as.publish_feedback(self._feedback)
plan = self.robot.closeGripper()
if plan is None:
rospy.loginfo("Close Gripper plan failed")
self._result.error_code.val = -1
self._as.set_aborted(self._result)
sucess = False
return None
self._result.trajectory_descriptions.append('CloseGripper')
self._result.trajectory_stages.append(plan)
self._feedback.state = "Closing gripper"
ex_status = self.robot.gripper_execute(plan)
if not ex_status:
rospy.loginfo("Execution to grasp failed: [%s]", ex_status)
self._result.error_code.val = -4
self._as.set_aborted(self._result)
sucess = False
return None
self._as.publish_feedback(self._feedback)
if sucess:
self._result.error_code.val = 1
self._as.set_succeeded(self._result)
def info_odom_callback(self, info, odom):
self.velocity[:] = []
self.V_direction[:] = []
self.V_magnitude[:] = []
if (info.detections.size > 0):
#rospy.loginfo("%s",info.detections.size)
# Store the path info for each detection and publish the path while getting the new message update.
for entry in info.detections.data:
if entry.num > self.paths_size:
for i in range(entry.num - self.paths_size):
self.paths.append(Path())
#self.dynamics.append(Dynamics())
self.paths_pub.append(
rospy.Publisher("~trajectory" +
str(self.paths_size + i + 1),
Path,
queue_size=1))
#self.dynamics_pub.append(rospy.Publisher("~dynamics")+str(self.paths_size + i + 1),Dynamics,queue_size = 1)
#self.pose_index.append(0)
self.paths_size = entry.num
pose = PoseStamped()
point_stamped = entry.ptStamped
pose.header = point_stamped.header
pose.pose.position = point_stamped.point
pose.pose.orientation.w = 1
try:
# Make true the frame_id of the pose (ptStamped)
self.trans_pose = self.listener.transformPose("map", pose)
except (tf.Exception):
rospy.loginfo(
"The transformation is not available right now!")
self.paths[entry.num - 1].poses.append(self.trans_pose)
self.paths[entry.num - 1].header.frame_id = 'map'
self.paths_pub[entry.num - 1].publish(self.paths[entry.num -
1])
#self.pose_index[entry.num - 1] += 1
for index in range(self.paths_size):
#self.velocity[:] = []
if len(self.paths[index].poses) > 2:
num = index + 1
pre_x = self.paths[index].poses[-2].pose.position.x
pre_y = self.paths[index].poses[-2].pose.position.y
pre_z = self.paths[index].poses[-2].pose.position.z
pre_stamp = self.paths[index].poses[-2].header.stamp
cur_x = self.paths[index].poses[-1].pose.position.x
cur_y = self.paths[index].poses[-1].pose.position.y
cur_z = self.paths[index].poses[-1].pose.position.z
cur_stamp = self.paths[index].poses[-1].header.stamp
duration = (cur_stamp - pre_stamp).to_sec()
vx = (cur_x - pre_x) / duration
vy = (cur_y - pre_y) / duration
self.velocity.append([cur_x, cur_y, cur_z, vx, vy, num])
else:
pass
odom_x = odom.pose.pose.position.x
odom_y = odom.pose.pose.position.y
odom_z = odom.pose.pose.position.z
'''odom_x = odom.height
odom_y = odom.width
odom_z = 1 '''
self.info = self.velocity.append([odom_x, odom_y,
odom_z]) # append
#rospy.loginfo("The current velocity info are %s",self.velocity)
if (len(self.velocity) > 2):
Human_num = len(self.velocity) - 1
for i in np.arange(Human_num):
self.Human_list.append([self.velocity[i][5]])
self.V_magnitude.append(
np.sqrt(
np.square(self.velocity[i][3]) +
np.square(self.velocity[i][4])))
vector1 = [self.velocity[i][3], self.velocity[i][4]]
vector2 = [
self.velocity[Human_num][0] - self.velocity[i][0],
self.velocity[Human_num][1] - self.velocity[i][1]
]
self.V_direction.append(
np.dot(vector1, vector2) /
(np.linalg.norm(vector1) * np.linalg.norm(vector2)))
Unique_v = self.crf(self.V_magnitude)
#print Unique_v
Unique_d = self.crf(self.V_direction)
#print Unique_d
Unique_d = self.normalize2(Unique_d)
#print Unique_d
#rospy.loginfo(len(self.V_direction))
#rospy.loginfo(len(self.V_magnitude))
Unique = self.normalize1(
np.array(Unique_v) * np.array(Unique_d))
Unique = list(Unique)
#print Unique
#print(len(self.velocity))
#rospy.loginfo(Unique.index(max(Unique)))
Unique_l = self.velocity[Unique.index(max(Unique))][5]
rospy.loginfo(
"The Abnormal Object's label detected in husky1 Thermal is %s",
Unique_l)
else:
rospy.logdebug(
"HUSKY1 (Thermal):There is no person detected in the current frame!"
)
magMsg.magnetic_field.z = float(words[11]) / 1000.0
else:
magMsg.magnetic_field.x = float('nan')
magMsg.magnetic_field.y = float('nan')
magMsg.magnetic_field.z = float('nan')
q = quaternion_from_euler(roll, pitch, yaw)
imuMsg.orientation.x = q[0]
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'imu_link'
imuMsg.header.seq = seq
magMsg.header = imuMsg.header
poseMsg.header = imuMsg.header
poseMsg.pose.position.x = poseMsg.pose.position.y = poseMsg.pose.position.z = 0.0
poseMsg.pose.orientation = imuMsg.orientation
seq = seq + 1
pub.publish(imuMsg)
magpub.publish(magMsg)
posepub.publish(poseMsg)
# br.sendTransform((0,0,0), tf.transformations.quaternion_from_euler(roll,pitch,yaw), imuMsg.header.stamp, 'imu_link','imu_frame')
if (diag_pub_time < rospy.get_time()):
diag_pub_time += 1
diag_arr = DiagnosticArray()
diag_arr.header.stamp = rospy.get_rostime()
diag_arr.header.frame_id = '1'
diag_msg = DiagnosticStatus()
diag_msg.name = 'Razor_Imu'
diag_msg.level = DiagnosticStatus.OK
def _update_grippers(self):
obj_im = self._im_server.get('object')
obj_ps = PoseStamped()
obj_ps.header = obj_im.header
obj_ps.pose = obj_im.pose
# yapf: disable
pregrasp_in_obj = np.array([
[1, 0, 0, -0.266],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1],
])
grasp_in_obj = np.array([
[1, 0, 0, -0.166],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1],
])
lift_in_obj = np.array([
[1, 0, 0, -0.166],
[0, 1, 0, 0],
[0, 0, 1, 0.2],
[0, 0, 0, 1],
])
# yapf: enable
pregrasp_pose = self._compute_grasp_offset(obj_ps, pregrasp_in_obj)
pregrasp_im = fetch_api.gripper_viz.gripper_interactive_marker(
pregrasp_pose, 0.1)
pregrasp_im.name = 'pregrasp'
joints = self._arm.compute_ik(pregrasp_pose)
if joints == False:
color_gripper(pregrasp_im, 1, 0, 0, 0.5)
else:
color_gripper(pregrasp_im, 0, 1, 0, 0.5)
grasp_pose = self._compute_grasp_offset(obj_ps, grasp_in_obj)
grasp_im = fetch_api.gripper_viz.gripper_interactive_marker(
grasp_pose, 0.1)
grasp_im.name = 'grasp'
joints = self._arm.compute_ik(grasp_pose)
if joints == False:
color_gripper(grasp_im, 1, 0, 0, 0.5)
else:
color_gripper(grasp_im, 0, 1, 0, 0.5)
lift_pose = self._compute_grasp_offset(obj_ps, lift_in_obj)
lift_im = fetch_api.gripper_viz.gripper_interactive_marker(
lift_pose, 0.1)
lift_im.name = 'lift'
joints = self._arm.compute_ik(lift_pose)
if joints == False:
color_gripper(lift_im, 1, 0, 0, 0.5)
else:
color_gripper(lift_im, 0, 1, 0, 0.5)
self._im_server.insert(pregrasp_im)
self._im_server.insert(grasp_im)
self._im_server.insert(lift_im)
self._im_server.applyChanges()
def update(self, m_array):
"""
Average all measurements
"""
if self.old_msg == m_array:
# Message old
return
self.old_msg = m_array
kalman_gains = []
filtered_poses = []
measured_valid_poses = PoseArray()
measured_invalid_poses = PoseArray()
measured_valid_poses.header.frame_id = "map"
measured_invalid_poses.header.frame_id = "map"
n_markers = len(m_array.markers)
for marker in m_array.markers:
# TODO: make this general (no hardcoded Qs)
if marker.id > 9:
frame_detected = "sign_detected/stop"
frame_marker = "sign/stop"
print("Using stop sign!!!")
Q = np.diag([0.5, 0.5, 0.5])
else:
frame_detected = "aruco/detected" + str(marker.id)
frame_marker = "aruco/marker" + str(marker.id)
Q = np.diag([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
try:
# just to get correct time stamp
time_stamp = self.tf_buf.lookup_transform(
frame_marker, frame_detected, rospy.Time(0)).header.stamp
except:
print("Wait a bit...")
return
measurement_fb = Int32MultiArray()
# could this fail?
believed_trans = self.tf_buf.lookup_transform(
"map", "cf1/base_link", time_stamp)
believed_pose = PoseStamped()
believed_pose.header = believed_trans.header
believed_pose.pose.position = Point(*[
believed_trans.transform.translation.x, believed_trans.
transform.translation.y, believed_trans.transform.translation.z
])
believed_pose.pose.orientation = believed_trans.transform.rotation
believed_state = self.pose_stamped_to_state(believed_pose)
measured_pose = self.get_measured_pose_filtered(
believed_pose, frame_marker, frame_detected)
measured_state = self.pose_stamped_to_state(measured_pose)
diff = self.kf.inovation(believed_state, measured_state)
maha_dist = self.kf.maha_dist(diff, Q)
print("Mahalanobis dist: {}".format(maha_dist))
if maha_dist > self.kf.maha_dist_thres:
# outlier
print("Outlier")
measured_invalid_poses.poses.append(measured_pose.pose)
measurement_fb.data = [marker.id, 0]
self.measurement_fb_pub.publish(measurement_fb)
continue
else:
measured_valid_poses.poses.append(measured_pose.pose)
measurement_fb.data = [marker.id, 1]
self.measurement_fb_pub.publish(measurement_fb)
K = self.kf.kalman_gain(Q)
kalman_gains.append(K)
filtered_state = self.filter_state(believed_state, measured_state,
K)
# for testing with K=1
#filtered_state = believed_state + self.kf.inovation(believed_state, measured_state)*1
filtered_pose = self.state_to_pose_stamped(
filtered_state, believed_pose.header.frame_id, time_stamp)
filtered_poses.append(filtered_pose)
self.measured_valid_pub.publish(measured_valid_poses)
self.measured_invalid_pub.publish(measured_invalid_poses)
print("Using {}/{} markers measurements".format(
len(filtered_poses), n_markers))
if len(filtered_poses) > 0:
K = sum(kalman_gains) / len(filtered_poses)
self.kf.update_with_gain(K)
filtered_pose = self.average_poses(filtered_poses)
self.filtered_pub.publish(filtered_pose) # for visualization
map_to_odom = self.get_map_to_odom(filtered_pose)
#print("Updated")
return map_to_odom
def __init__(self):
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("manipulator")
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.goal_state_publisher = rospy.Publisher(
'/rviz/moveit/update_custom_goal_state',
moveit_msgs.msg.RobotState,
queue_size=20)
# Walls are defined with respect to the coordinate frame of the robot base, with directions
# corresponding to standing behind the robot and facing into the table.
rospy.sleep(0.6)
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'world'
# self.robot.get_planning_frame()
table_pose = PoseStamped()
table_pose.header = header
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0
table_pose.pose.position.z = 0.02 # -0.0001
self.scene.remove_world_object('table')
self.scene.add_plane(name='table', pose=table_pose, normal=(0, 0, 1))
upper_pose = PoseStamped()
upper_pose.header = header
upper_pose.pose.position.x = 0
upper_pose.pose.position.y = 0
upper_pose.pose.position.z = 0.62 # Optimized (0.55 NG)
self.scene.remove_world_object('upper')
self.scene.add_plane(name='upper', pose=upper_pose, normal=(0, 0, 1))
back_pose = PoseStamped()
back_pose.header = header
back_pose.pose.position.x = 0
back_pose.pose.position.y = -0.25
back_pose.pose.position.z = 0
self.scene.remove_world_object('backWall')
self.scene.add_plane(name='backWall', pose=back_pose, normal=(0, 1, 0))
right_pose = PoseStamped()
right_pose.header = header
right_pose.pose.position.x = 0.5 # 0.2
right_pose.pose.position.y = 0
right_pose.pose.position.z = 0
self.scene.remove_world_object('rightWall')
self.scene.add_plane(name='rightWall',
pose=right_pose,
normal=(1, 0, 0))
left_pose = PoseStamped()
left_pose.header = header
left_pose.pose.position.x = -0.54
left_pose.pose.position.y = 0
left_pose.pose.position.z = 0
self.scene.remove_world_object('leftWall')
self.scene.add_plane(name='leftWall', pose=left_pose, normal=(1, 0, 0))
rospy.sleep(0.6)
# rospy.loginfo(self.scene.get_known_object_names())
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = self.group.get_planning_frame()
# print "============ Reference frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = self.group.get_end_effector_link()
# print "============ End effector: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = self.robot.get_group_names()
# print "============ Robot Groups:", self.robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
# print "============ Printing robot state"
# print self.robot.get_current_state()
# print ""
self.group.set_max_acceleration_scaling_factor(0.3)
self.group.set_max_velocity_scaling_factor(0.3)
def test_depth_mapping():
pose_pub = rospy.Publisher(pose_topic, PoseStamped)
path_pub = rospy.Publisher(path_topic, Path)
rospy.init_node('mapping', anonymous=True)
rospy.loginfo("Start Mapping")
mapping = depth_mapping.Mapping()
params = net.monodepth_main.monodepth_parameters(
encoder='vgg',
height=256,
width=512,
batch_size=8,
num_threads=8,
num_epochs=50,
do_stereo=False,
wrap_mode='border',
use_deconv=False,
alpha_image_loss=0.85,
disp_gradient_loss_weight=0.1,
lr_loss_weight=1.0,
full_summary=True)
root_path = '/home/chen-tian/data/data/code/learningReloc/'
data_path = '/home/chen-tian/data/data/KITTI/odom/'
test_params = net.utils.utils.test_parameters(
root_path=root_path,
data_path=data_path,
filenames_file=root_path +
'net/utils/filenames//kitti_odom_color_depth.txt',
dataset='kitti',
mode='test',
checkpoint_path=root_path + 'net/data/model/model_kitti',
log_directory=root_path + 'learningReloc/log/',
output_directory=root_path + 'learningReloc/output/',
kitti_calib=data_path + 'dataset/sequences/00/calib.txt',
trajectory_file=data_path + 'dataset/poses/00.txt',
height_origin=370,
width_origin=1226,
calib_ext_file='',
calib_int_file='',
ground_truth_image='')
num_test_samples = mapping.build_net(params, test_params)
mapping.load_trajectory(test_params.trajectory_file)
mapping.calib_params(test_params.kitti_calib)
assert num_test_samples == len(mapping.trajectory)
print('now testing {} files'.format(num_test_samples))
# disparities_vector = np.zeros((num_test_samples, params.height, params.width), dtype=np.float32)
# disparities_pp_vector = np.zeros((num_test_samples, params.height, params.width), dtype=np.float32)
path_msg = Path()
for step in range(num_test_samples):
disp, left_image = mapping.sess_run()
disparities = disp[0].squeeze()
plt.imshow(disparities)
plt.pause(0.001)
left_ori = np.uint8(left_image[0] * 255)
depth = mapping.bf / disparities
cv2.imshow('left', left_ori)
cv2.waitKey(10)
color_origin = np.uint8(left_image[0] * 255)
gray_orgin = cv2.cvtColor(color_origin, cv2.COLOR_BGR2GRAY)
pose_mat = np.eye(4)
pose_mat[:3, :] = mapping.trajectory[step].reshape((3, 4))
pcd = net.utils.utils.triangulate(
gray_orgin, depth, pose_mat, mapping.intrinsic,
(test_params.width_origin, test_params.height_origin))
cloud = PointCloud()
cloud.header = std_msgs.msg.Header()
cloud.header.stamp = rospy.Time.now()
cloud.header.frame_id = "mapping"
point_num = len(pcd)
cloud.points = [None] * point_num
for i in range(point_num):
x, y, z, w = pcd[i]
cloud.points[i] = Point(x, y, z)
cloud_pub.publish(cloud)
pose_msg = PoseStamped()
pose_msg.pose.position.x = pose_mat[0, 3]
pose_msg.pose.position.y = pose_mat[1, 3]
pose_msg.pose.position.z = pose_mat[2, 3]
w, x, y, z = data.pose_utils.rot2quat(pose_mat[:3, :3])
pose_msg.pose.orientation.w = w
pose_msg.pose.orientation.x = x
pose_msg.pose.orientation.y = y
pose_msg.pose.orientation.z = z
pose_msg.header = std_msgs.msg.Header()
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = 'mapping'
pose_pub.publish(pose_msg)
path_msg.header = pose_msg.header
path_msg.poses.append(pose_msg)
path_pub.publish(path_msg)
rospy.sleep(0.001)
def readiPose(msg):
print "Converting Pose"
convertedPose = PoseStamped()
convertedPose.header = msg.header
convertedPose.pose = msg.pose.pose
pose_stamped_pub.publish(convertedPose)
def icf(self):
dt = rospy.get_time() - self.prev_time
if dt > 1.0:
print "DT BIG: %f" % (dt)
#print dt
self.prev_time = rospy.get_time()
if self.x_post is not None:
for i in range(self.Ncars):
self.xi_prior[i*self.Ndim:(i+1)*self.Ndim] = self.robot.state_transition(
self.x_post[i*self.Ndim:(i+1)*self.Ndim], self.u[i], dt)
phi = self.robot.phi(self.x_post, self.u, dt, self.Ncars, self.Ndim)
self.Ji_prior = np.linalg.inv(phi*np.linalg.inv(self.J_post)*phi.T + self.Q)
self.Vi = self.Ji_prior/self.Ncars + np.dot(self.Hi.T,np.dot(self.Bi,self.Hi))
self.vi = np.dot(self.Ji_prior,self.xi_prior)/self.Ncars \
+ np.dot(self.Hi.T,np.dot(self.Bi,self.zi))
self.current_counter = 0
self.vj = {}
self.Vj = {}
st0 = rospy.get_time()
while self.current_counter < self.K and not rospy.is_shutdown():
self.publish_v()
if len(self.vj) == self.Nconn -1 and len(self.Vj) == self.Nconn-1:
self.vi, self.Vi = self.consensus(self.vi, self.Vi, self.vj, self.Vj)
self.vj = {}
self.Vj = {}
self.current_counter += 1
else:
self.rate.sleep()
# this is meant to break out of the loop
# if you kill the node
# if rospy.get_time() - st0 > 2.0:
# break
tim3 = rospy.get_time() - st0
# print "consensus loops: %f" % (tim3)
self.x_post = np.dot(np.linalg.inv(self.Vi),self.vi)
self.J_post = self.Ncars*self.Vi
cs = CombinedState()
cs.header = Header()
cs.header.frame_id = "map"
cs.header.stamp = rospy.Time().now()
cs.state = self.x_post.flatten().tolist()
self.consensus_state_pub.publish(cs)
consensus_poses = PoseArray()
consensus_poses.header.frame_id = "map"
consensus_poses.header.stamp = rospy.Time.now()
for i in range(self.Ncars):
pose = PoseStamped()
pose.header = Header()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time().now()
pose.pose.position.x = self.x_post[i*self.Ndim]
pose.pose.position.y = self.x_post[i*self.Ndim + 1]
theta = tf.transformations.quaternion_from_euler(0, 0, self.x_post[i*self.Ndim+2])
pose.pose.orientation.x = theta[0]
pose.pose.orientation.y = theta[1]
pose.pose.orientation.z = theta[2]
pose.pose.orientation.w = theta[3]
consensus_poses.poses.append(pose.pose)
self.br.sendTransform((self.x_post[i*self.Ndim], self.x_post[i*self.Ndim + 1], 0),
theta, rospy.Time.now(),
"car" + str(self.car_id) + "/car" + str(i) + "/base_link",
"map")
self.paths[i].poses.append(pose)
if len(self.paths[i].poses) > 300:
self.paths[i].poses.pop(0)
self.consensus_pub[i].publish(self.paths[i])
self.consensus_pose_pub.publish(consensus_poses)
def create_robot_pose():
pose = PoseStamped()
pose.pose.position.x = 20
pose.pose.position.y = 240
pose.header = "map"
return pose
def Odometry_callback(self, odom_msg):
pose_msg = PoseStamped()
pose_msg.header = odom_msg.header
pose_msg.pose = odom_msg.pose.pose
self.pose_pub.publish(pose_msg)
def ctrl_callback(self, ctrl_flag_msg):
print('----ctrl callback----')
pose = self.pose.copy()
pose[2] = self.normalize(pose[2])
self.old_obsv = np.array(self.curr_obsv, copy=True)
self.curr_obsv = np.array(self.obsv, copy=True)
update_flag = np.array_equal(self.old_obsv, self.curr_obsv)
########
# landmarks table test
########
num_lm = int(0.5 * (self.ekf_mean.shape[0] - 3))
self.obsv_table = []
tid = 0
for olm in self.curr_obsv:
# for each observation
'''
tflag = 1
for lid in range(num_lm):
# compare it with observed landmark
tlm = self.ekf_mean[3+lid*2:5+lid*2]
lm_diff = np.sum((olm-tlm)**2)
if lm_diff < 0.3:
# this is observed landmark
# self.ekf_mean[3+lid*2] = olm[0]
# self.ekf_mean[4+lid*2] = olm[1]
self.obsv_table.append(lid)
tflag = 0
break
else:
pass
'''
ds_score = []
for lid in range(num_lm):
# compare it with observed landmarks
tlm = self.ekf_mean[3 + lid * 2:5 + lid * 2]
lm_diff = np.sqrt(np.sum((olm - tlm)**2))
ds_score.append(lm_diff)
if len(ds_score) > 0:
if min(ds_score) > 0.4: # new landmark
self.obsv_table.append(num_lm + tid)
self.ekf_mean = np.concatenate((self.ekf_mean, olm))
self.ekf_cov = np.block(
[[self.ekf_cov,
np.zeros((self.ekf_cov.shape[0], 2))],
[
np.zeros((2, self.ekf_cov.shape[0])),
np.eye(2) * 1000
]])
tid += 1
else:
lidx = np.argmin(ds_score)
self.obsv_table.append(lidx)
else:
self.obsv_table.append(num_lm + tid)
self.ekf_mean = np.concatenate((self.ekf_mean, olm))
self.ekf_cov = np.block(
[[self.ekf_cov,
np.zeros((self.ekf_cov.shape[0], 2))],
[np.zeros((2, self.ekf_cov.shape[0])),
np.eye(2) * 1000]])
tid += 1
print('obsv: {}'.format(self.curr_obsv))
print('obsv table: {}'.format(self.obsv_table))
cube_list = Marker()
cube_list.header.frame_id = 'odom'
cube_list.header.stamp = rospy.Time.now()
cube_list.ns = 'landmark_map'
cube_list.action = Marker.ADD
cube_list.pose.orientation.w = 1.0
cube_list.id = 0
cube_list.type = Marker.CUBE_LIST
cube_list.scale.x = 0.05
cube_list.scale.y = 0.05
cube_list.scale.z = 0.5
cube_list.color.r = 1.0
cube_list.color.g = 1.0
cube_list.color.a = 1.0
for i in range(num_lm + tid):
landmark = self.ekf_mean[3 + i * 2:5 + i * 2]
p = Point()
p.x = landmark[0]
p.y = landmark[1]
p.z = 0.25
cube_list.points.append(p)
self.map_pub.publish(cube_list)
########
# ekf
########
prev_ctrl = np.array([0., 0.])
if len(self.log['ctrls']) > 0:
prev_ctrl = self.log['ctrls'][-1].copy()
G = np.eye(self.ekf_mean.shape[0])
G[0][2] = -np.sin(self.ekf_mean[2]) * prev_ctrl[0] * 0.1
G[1][2] = np.cos(self.ekf_mean[2]) * prev_ctrl[1] * 0.1
num_lm = int(0.5 * (self.ekf_mean.shape[0] - 3))
BigR = np.block(
[[self.ekf_R, np.zeros((3, 2 * num_lm))],
[np.zeros((2 * num_lm, 3)),
np.zeros((2 * num_lm, 2 * num_lm))]])
self.ekf_cov = G @ self.ekf_cov @ G.T + BigR
# self.ekf_mean[0] = pose[0]
# self.ekf_mean[1] = pose[1]
# self.ekf_mean[2] = pose[2]
# self.ekf_mean[2] = self.normalize(self.ekf_mean[2])
self.ekf_mean[0] += np.cos(self.ekf_mean[2]) * prev_ctrl[0] * 0.1
self.ekf_mean[1] += np.sin(self.ekf_mean[2]) * prev_ctrl[0] * 0.1
self.ekf_mean[2] += prev_ctrl[1] * 0.1
self.ekf_mean[2] = self.normalize(self.ekf_mean[2])
# if update_flag is False:
if True:
num_obsv = len(self.curr_obsv)
H = np.zeros((2 * num_obsv, self.ekf_mean.shape[0]))
r = self.ekf_mean[0:3].copy()
ref_obsv = []
for i in range(num_obsv):
idx = i * 2
lid = self.obsv_table[i]
lm = self.ekf_mean[3 + lid * 2:5 + lid * 2]
zr = np.sqrt((r[0] - lm[0])**2 + (r[1] - lm[1])**2)
H[idx][0] = (r[0] - lm[0]) / zr
H[idx][1] = (r[1] - lm[1]) / zr
H[idx][2] = 0
H[idx][3 + 2 * lid] = -(r[0] - lm[0]) / zr
H[idx][4 + 2 * lid] = -(r[1] - lm[1]) / zr
H[idx + 1][0] = -(r[1] - lm[1]) / zr**2
H[idx + 1][1] = (r[0] - lm[0]) / zr**2
H[idx + 1][2] = -1
H[idx + 1][3 + 2 * lid] = (r[1] - lm[1]) / zr**2
H[idx + 1][4 + 2 * lid] = -(r[0] - lm[0]) / zr**2
ref_obsv.append(self.range_bearing(r, lm))
ref_obsv = np.array(ref_obsv)
BigQ = linalg.block_diag(*[self.ekf_Q for _ in range(num_obsv)])
mat1 = np.dot(self.ekf_cov, H.T)
mat2 = np.dot(np.dot(H, self.ekf_cov), H.T)
mat3 = np.linalg.inv(mat2 + BigQ)
K = np.dot(mat1, mat3)
ori_obsv = []
for obsv in self.curr_obsv:
temp = self.range_bearing(r, obsv)
temp[1] = self.normalize(temp[1])
ori_obsv.append(temp.copy())
ori_obsv = np.array(ori_obsv)
# raw_scan = np.array(self.raw_scan)
if len(ori_obsv) == 0:
pass
else:
# raw_scan[:,1] = self.normalize(raw_scan[:,1])
delta_z = ori_obsv - ref_obsv
delta_z[:, 1] = self.normalize(delta_z[:, 1])
delta_z = delta_z.reshape(-1)
self.ekf_mean += K @ delta_z
self.ekf_cov -= K @ H @ self.ekf_cov
print('r: {} | {}'.format(pose, self.ekf_mean[0:3]))
print(len(self.ekf_mean))
ekf_odom = Odometry()
ekf_odom.header = copy(self.odom_header)
ekf_odom.child_frame_id = "base_footprint"
ekf_odom.pose.pose.position.x = self.ekf_mean[0]
ekf_odom.pose.pose.position.y = self.ekf_mean[1]
ekf_odom.pose.covariance[0] = self.ekf_cov[0][0]
ekf_odom.pose.covariance[1] = self.ekf_cov[1][1]
self.ekf_pub.publish(ekf_odom)
print('ekf cov: ', np.trace(self.ekf_cov))
########
# ctrl
########
idx = self.log['count'] % 10
self.erg_ctrl.barr.update_obstacles(self.obsv)
# _, ctrl_seq = self.erg_ctrl(pose.copy(), seq=True)
_, ctrl_seq = self.erg_ctrl(self.ekf_mean[0:3].copy(), seq=True)
if idx == 0:
print('update ctrl seq')
self.ctrl_seq = ctrl_seq.copy()
ctrl = self.ctrl_seq[idx]
ctrl_lin = ctrl[0]
ctrl_ang = ctrl[1]
vel_msg = Twist()
vel_msg.linear.x = ctrl_lin
vel_msg.linear.y = 0.0
vel_msg.linear.z = 0.0
vel_msg.angular.x = 0.0
vel_msg.angular.y = 0.0
vel_msg.angular.z = ctrl_ang
self.ctrl_pub.publish(vel_msg)
else:
print('follow ctrl seq')
ctrl = self.ctrl_seq[idx]
ctrl_lin = ctrl[0]
ctrl_ang = ctrl[1]
vel_msg = Twist()
vel_msg.linear.x = ctrl_lin
vel_msg.linear.y = 0.0
vel_msg.linear.z = 0.0
vel_msg.angular.x = 0.0
vel_msg.angular.y = 0.0
vel_msg.angular.z = ctrl_ang
self.ctrl_pub.publish(vel_msg)
# log
self.log['count'] += 1
self.log['traj'].append(pose.copy())
self.log['ctrls'].append(ctrl.copy())
# publish predicted trajectory
self.path_msg = Path()
self.path_msg.header = copy(self.odom_header)
dummy_pose = pose.copy()
for i in range(idx, 80):
dummy_ctrl = self.ctrl_seq[i]
dummy_pose += 0.1 * np.array([
cos(dummy_pose[2]) * dummy_ctrl[0],
sin(dummy_pose[2]) * dummy_ctrl[0], dummy_ctrl[1]
])
pose_msg = PoseStamped()
pose_msg.header = copy(self.odom_header)
pose_msg.pose.position.x = dummy_pose[0]
pose_msg.pose.position.y = dummy_pose[1]
self.path_msg.poses.append(copy(pose_msg))
self.path_pub.publish(self.path_msg)
def handle_plan_request(self, req):
""" Callback function for a grasp planning servce request. """
# TODO generate HFTS from point cloud if point cloud is specified
# pointCloud = req.point_cloud
rospy.loginfo('Executing planner with parameters: ' +
str(self._params))
# Load the requested object first
# TODO setting this boolean parameter should be solved in a more elegant manner
self._object_loader._b_var_filter = self._params['hfts_filter_points']
self._planner.load_object(req.object_identifier)
hfts_gen_params = {
'max_normal_variance':
self._params['max_normal_variance'],
'contact_density':
self._params['contact_density'],
'min_contact_patch_radius':
self._params['min_contact_patch_radius'],
'max_num_points':
self._params['max_num_points'],
'position_weight':
self._params['hfts_position_weight'],
'branching_factor':
self._params['hfts_branching_factor'],
'first_level_branching_factor':
self._params['hfts_first_level_branching_factor']
}
self._planner.set_parameters(
max_iters=self._params['num_hfts_iterations'],
reachability_weight=self._params['reachability_weight'],
com_center_weight=self._params['com_center_weight'],
hfts_generation_params=hfts_gen_params,
b_force_new_hfts=self._params['force_new_hfts'])
# We always start from the root node, so create a root node
root_hfts_node = HFTSNode()
num_planning_attempts = self._params['num_planning_attempts']
rospy.loginfo(
'[HandlerClass::handle_plan_request] Planning grasp, running %i attempts.'
% num_planning_attempts)
iteration = 0
# Iterate until either shutdown, max_iterations reached or a good grasp was found
while iteration < num_planning_attempts and not rospy.is_shutdown():
return_node = self._planner.sample_grasp(
root_hfts_node,
self._planner.get_maximum_depth(),
post_opt=True)
iteration += 1
if return_node.is_goal():
rospy.loginfo(
'[HandlerClass::handle_plan_request] Found a grasp after %i attempts.'
% iteration)
grasp_pose = return_node.get_hand_transform()
pose_quaternion = tff.quaternion_from_matrix(grasp_pose)
pose_position = grasp_pose[:3, -1]
# Save pose in ROS pose
ros_grasp_pose = Pose()
ros_grasp_pose.position.x = pose_position[0]
ros_grasp_pose.position.y = pose_position[1]
ros_grasp_pose.position.z = pose_position[2]
ros_grasp_pose.orientation.x = pose_quaternion[0]
ros_grasp_pose.orientation.y = pose_quaternion[1]
ros_grasp_pose.orientation.z = pose_quaternion[2]
ros_grasp_pose.orientation.w = pose_quaternion[3]
# Make a header for the message
header = Header()
header.frame_id = req.object_identifier
header.stamp = rospy.Time.now()
# Create stamped pose
stamped_ros_grasp_pose = PoseStamped()
stamped_ros_grasp_pose.pose = ros_grasp_pose
stamped_ros_grasp_pose.header = header
# Create JointState message to send hand configuration
hand_conf = return_node.get_hand_config()
ros_hand_joint_state = JointState()
ros_hand_joint_state.header = header
ros_hand_joint_state.position = hand_conf
ros_hand_joint_state.name = self._joint_names
# Return the response
return PlanGraspResponse(True, stamped_ros_grasp_pose,
ros_hand_joint_state)
# In case of failure or shutdown return a response indicating failure.
rospy.loginfo(
'[HandlerClass::handle_plan_request] Failed to find a grasp.')
return PlanGraspResponse(False, PoseStamped(), JointState())
def default(self, ci='unused'):
if 'valid' not in self.data or self.data['valid']:
pose = PoseStamped()
pose.header = self.get_ros_header()
pose.pose = get_pose(self)
self.publish(pose)
def fiducial_cb(self, msg):
if len(msg.transforms) > 0:
goal = PoseStamped()
goal.header = msg.header
goal.pose.position.x = msg.transforms[0].transform.translation.x
goal.pose.position.y = msg.transforms[0].transform.translation.y
goal.pose.position.z = msg.transforms[0].transform.translation.z
goal.pose.orientation = msg.transforms[0].transform.rotation
transform = self.tf_buffer.lookup_transform(
"map",
goal.header.frame_id, #source frame
rospy.Time(0), #get the tf at first available time
rospy.Duration(1.0)) #wait for 1 second
goal_transformed = tf2_geometry_msgs.do_transform_pose(
goal, transform)
if self.locked_goal is None:
self.sent_cmd_to_move_base(goal_transformed)
rospy.loginfo("[aruco_searcher] goal sent.")
else:
dx = self.locked_goal.pose.position.x - goal_transformed.pose.position.x
dy = self.locked_goal.pose.position.y - goal_transformed.pose.position.y
dr = np.sqrt(dx**2 + dy**2)
if dr > self.dr_threshold:
rospy.loginfo(
"[aruco_searcher] dr ({}) is higher then dr_threshold ({}), goal resent."
.format(dr, self.dr_threshold))
self.sent_cmd_to_move_base(goal_transformed)
else:
quat_tf_1 = [
self.locked_goal.pose.orientation.x,
self.locked_goal.pose.orientation.y,
self.locked_goal.pose.orientation.z,
self.locked_goal.pose.orientation.w
]
quat_tf_2 = [
goal_transformed.pose.orientation.x,
goal_transformed.pose.orientation.y,
goal_transformed.pose.orientation.z,
goal_transformed.pose.orientation.w
]
yaw1 = euler_from_quaternion(quat_tf_1)[2]
yaw2 = euler_from_quaternion(quat_tf_2)[2]
dyaw = abs(
np.arctan2(np.sin(yaw1 - yaw2), np.cos(yaw1 - yaw2)))
if dyaw > self.dyaw_threshold:
rospy.loginfo(
"[aruco_searcher] dyaw ({}) is higher then dyaw_threshold ({}), goal resent."
.format(dyaw, self.dyaw_threshold))
self.sent_cmd_to_move_base(goal_transformed)
def callback(data):
# trans2 = tf_buffer.lookup_transform('r2_link_0',
# 'camera_link',
# rospy.Time(0),
# rospy.Duration(1.0))
trans2 = tf_buffer.lookup_transform('r2_link_0',
'camera_color_optical_frame',
rospy.Time(0),
rospy.Duration(1.0))
if len(data.markers)>0:
for i in range(0,len(data.markers)):
poseS = PoseStamped ()
poseS.pose = data.markers[i].pose
poseS.header = data.header
pose_transformed_obj = tf2_geometry_msgs.do_transform_pose(poseS, trans2)
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", pose_transformed_obj)
pose_transformed_obj.header = poseS.header
rospy.loginfo(rospy.get_caller_id() + "I heard %s", pose_transformed_obj)
if data.markers[i].ids[0] in markers.keys():
#append new marker detection to collection
markers[data.markers[i].ids[0]].append(pose_transformed_obj)
else:
#if the marker was not detected so far
#markers will be stored in the collection with buffer of 20 detections (approx.1 second)
#TODO check proper size of buffer, this will slow down the real-time process
markers[data.markers[i].ids[0]] = collections.deque(maxlen = 20)
markers[data.markers[i].ids[0]].append(pose_transformed_obj)
else:
print('No markers detected')
dataM = []
for key, item in markers.items():
duration = (item[-1].header.stamp-item[0].header.stamp).to_sec()
duration2 = (rospy.Time.now()-item[-1].header.stamp).to_sec()
distance = ((100 * (item[-1].pose.position.x - item[0].pose.position.x)) ** 2 + (
100 * (item[-1].pose.position.y - item[0].pose.position.y)) ** 2 + (
100 * (item[-1].pose.position.z - item[0].pose.position.z)) ** 2)
# print('key: {}, number of poses: {}, duration last to first pose: {}'.format(key, len(item), duration))
# print('duration {}'.format(duration2))
#TODO add duration and distance from current time (historical data we want to throw away)
#TODO add constraint on distance (not moving objects counted)
#if number of items for given marker is 20 within last N seconds and not too old data, save its average
if (((len(item) == 20 and duration < 10) and (duration2<10)) and distance < 1):
marker_position = []
marker_orientation = np.array([])
for poses in item:
#orientation has to be via np.stack as a correct input to quaternion_averaging - needs correct shape
if (np.shape(marker_orientation)[0] > 0):
marker_orientation = np.vstack(
(marker_orientation, [poses.pose.orientation.x, poses.pose.orientation.y,
poses.pose.orientation.z, poses.pose.orientation.w]))
else:
marker_orientation = np.array([poses.pose.orientation.x, poses.pose.orientation.y,
poses.pose.orientation.z, poses.pose.orientation.w])
marker_position.append([poses.pose.position.x,
poses.pose.position.y, poses.pose.position.z])
#print('quaternion avg {}'.format(quaternion_averaging.averageQuaternions(marker_orientation)))
#print('position avg {}'.format(sum(np.array(marker_position)) / np.size(np.array(marker_position))))
markers_avg[key] = {}
markers_avg[key]['orientation'] = quaternion_averaging.averageQuaternions(marker_orientation).tolist()
div = np.size(np.array(marker_position))
markers_avg[key]['position'] = sum(np.array(marker_position),0) / np.size(np.array(marker_position),0)
dataM1 = MarkersAvg()
dataM1.id = key
dataM1.avg_pose.position.x = markers_avg[key]['position'][0]
dataM1.avg_pose.position.y = markers_avg[key]['position'][1]
dataM1.avg_pose.position.z = markers_avg[key]['position'][2]
dataM1.avg_pose.orientation.x = markers_avg[key]['orientation'][0]
dataM1.avg_pose.orientation.y = markers_avg[key]['orientation'][1]
dataM1.avg_pose.orientation.z = markers_avg[key]['orientation'][2]
dataM1.avg_pose.orientation.w = markers_avg[key]['orientation'][3]
if dataM1.avg_pose.position.x < 0.4:
print('hallo')
dataM.append(dataM1)
print('markers average {}'.format(markers_avg))
global pub
if pub is None:
pub = rospy.Publisher('/averaged_markers', MarkersAvgList, queue_size=10)
msg = MarkersAvgList ()
# msg.header.stamp = rospy.Time.now()
if pose_transformed_obj:
msg.header = pose_transformed_obj.header
else:
msg.header.frame_id = trans2.header.frame_id # pose_transformed_obj.header
msg.header.stamp = rospy.Time.now()
msg.markers = dataM
#rospy.init_node('averaged_markers', anonymous=True)
# rate = rospy.Rate(10) # 10hz
if len(dataM) == 0:
return
pub.publish(msg)
def poseCallback(msg):
poseStpd = PoseStamped()
poseStpd.header = msg.header
poseStpd.pose.position = msg.pose.pose.position
poseStpd.pose.orientation = msg.pose.pose.orientation
currnetPosePublisher.publish(poseStpd)
def global_input_cb(self, msg):
self.force_monitor.update_activity()
if self.is_forced_retreat:
return
rospy.loginfo("[face_adls_manager] Performing global move.")
if type(msg) == String:
self.publish_feedback(Messages.GLOBAL_START % msg.data)
goal_ell_pose = self.global_poses[msg.data]
elif type(msg) == PoseStamped:
try:
self.tfl.waitForTransform(msg.header.frame_id,
'/ellipse_frame', msg.header.stamp,
rospy.Duration(8.0))
goal_cart_pose = self.tfl.transformPose('/ellipse_frame', msg)
goal_cart_pos, goal_cart_quat = PoseConv.to_pos_quat(
goal_cart_pose)
flip_quat = trans.quaternion_from_euler(-np.pi / 2, np.pi, 0)
goal_cart_quat_flipped = trans.quaternion_multiply(
goal_cart_quat, flip_quat)
goal_cart_pose = PoseConv.to_pose_stamped_msg(
'/ellipse_frame', (goal_cart_pos, goal_cart_quat_flipped))
self.publish_feedback(
'Moving around ellipse to clicked position).')
goal_ell_pose = self.ellipsoid.pose_to_ellipsoidal(
goal_cart_pose)
except (LookupException, ConnectivityException,
ExtrapolationException, Exception) as e:
rospy.logwarn(
"[face_adls_manager] TF Failure getting clicked position in ellipse_frame:\r\n %s"
% e)
return
curr_cart_pos, curr_cart_quat = self.current_ee_pose(
self.ee_frame, '/ellipse_frame')
curr_ell_pos, curr_ell_quat = self.ellipsoid.pose_to_ellipsoidal(
(curr_cart_pos, curr_cart_quat))
retreat_ell_pos = [curr_ell_pos[0], curr_ell_pos[1], RETREAT_HEIGHT]
retreat_ell_quat = trans.quaternion_multiply(self.gripper_rot,
[1., 0., 0., 0.])
approach_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1], RETREAT_HEIGHT
]
approach_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
final_ell_pos = [
goal_ell_pose[0][0], goal_ell_pose[0][1],
goal_ell_pose[0][2] - HEIGHT_CLOSER_ADJUST
]
final_ell_quat = trans.quaternion_multiply(self.gripper_rot,
goal_ell_pose[1])
cart_ret_pose = self.ellipsoid.ellipsoidal_to_pose(
(retreat_ell_pos, retreat_ell_quat))
cart_ret_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_ret_pose)
cart_app_pose = self.ellipsoid.ellipsoidal_to_pose(
(approach_ell_pos, approach_ell_quat))
cart_app_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_app_pose)
cart_goal_pose = self.ellipsoid.ellipsoidal_to_pose(
(final_ell_pos, final_ell_quat))
cart_goal_msg = PoseConv.to_pose_stamped_msg('ellipse_frame',
cart_goal_pose)
retreat_ell_traj = self.ellipsoid.create_ellipsoidal_path(
curr_ell_pos,
curr_ell_quat,
retreat_ell_pos,
retreat_ell_quat,
velocity=0.01,
min_jerk=True)
transition_ell_traj = self.ellipsoid.create_ellipsoidal_path(
retreat_ell_pos,
retreat_ell_quat,
approach_ell_pos,
approach_ell_quat,
velocity=0.01,
min_jerk=True)
approach_ell_traj = self.ellipsoid.create_ellipsoidal_path(
approach_ell_pos,
approach_ell_quat,
final_ell_pos,
final_ell_quat,
velocity=0.01,
min_jerk=True)
full_ell_traj = retreat_ell_traj + transition_ell_traj + approach_ell_traj
full_cart_traj = [
self.ellipsoid.ellipsoidal_to_pose(pose) for pose in full_ell_traj
]
cart_traj_msg = [PoseConv.to_pose_msg(pose) for pose in full_cart_traj]
head = Header()
head.frame_id = '/ellipse_frame'
head.stamp = rospy.Time.now()
pose_array = PoseArray(head, cart_traj_msg)
self.pose_traj_pub.publish(pose_array)
ps = PoseStamped()
ps.header = head
ps.pose = cart_traj_msg[0]
self.force_monitor.update_activity()
def __init__(self):
print('Start node')
# Initialize node
rospy.init_node('ellipse_publisher', anonymous=True)
rate = rospy.Rate(2) # Frequency
self.n_obs = 1
# Create publishers
pub_traj = rospy.Publisher('ds_finalTrajectory', Path, queue_size=5)
pub_pos1 = rospy.Publisher('pose_obstacle', PoseStamped, queue_size=5)
# Create listener
pose_sub = [0,0]
pose_sub[0] = rospy.Subscriber("obstacle0", Obstacle, self.callback_ob0)
pose_sub[1] = rospy.Subscriber("obstacle1", Obstacle, self.callback_ob1)
attr_sub = rospy.Subscriber("attr", Obstacle, self.callback_ob1)
self.listener = tf.TransformListener() # TF listener
self.n_obs = 1
self.obs = [0]*self.n_obs
self.pos_obs=[0]*self.n_obs
self.quat_ob=[0]*self.n_obs
print('wait obstacle')
self.awaitObstacle = [True for i in range(self.n_obs)]
while any(self.awaitObstacle):
rospy.sleep(0.1) # Wait zzzz* = 1 # wait
print('got it')
# Get initial transformation
rospy.loginfo('Getting Transformationss.')
awaitingTrafo_ee=True
awaitingTrafo_obs[i]= [True] * self.obs
awaitingTrafo_attr=True
for i in len(self.obs):
while(awaitingTrafo_obs[i]): # Trafo obs1 position
try:
self.pos_obs[i], self.quat_obs[i] = self.listener.lookupTransform("/mocap_world", "/object_{}/base_link".format(i), rospy.Time(0))
awaitingTrafo_obs[i]=False
except:
rospy.loginfo('Waiting for TRAFO at {}'.format(rospy.get_time()))
rospy.sleep(0.1) # Wait zzzz*
while(awaitingTrafo_ee): # TRAFO robot position
try: # Get transformation
self.pos_rob, self.pos_rob = self.listener.lookupTransform("/mocap_world", "/lwr_7_link", rospy.Time(0))
awaitingTrafo_ee=False
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo('Waiting for TRAFO')
rospy.sleep(0.1) # Wait zzzz*
while(awaitingTrafo_attr): # Trafo obs2 position
try:
self.pos_attr, self.quat_attr = self.listener.lookupTransform("/mocap_world", "/object_2/base_link", rospy.Time(0))
awaitingTrafo_attr=False
except:
rospy.loginfo("Waiting for TRAFO at {}".format(rospy.get_time()))
rospy.sleep(0.5) # Wait zzzz*
rospy.loginfo("All TF recieved")
self.iSim = 0
# Variables for trajectory prediciton
self.n_intSteps = 100
self.dt = 0.05
self.dim = 3 # 3D space
while not rospy.is_shutdown():
try: # Get transformation
self.pos_rob, self.quat_rob = self.listener.lookupTransform("/mocap_world", "/lwr_7_link", rospy.Time(0))
except:
rospy.loginfo("No <</lwr_7_link>> recieved")
#continue
x0_lwr7 = np.array([0,0,0])+np.array(self.pos_rob)
try: # Get transformation
self.pos_attr, self.attr = self.listener.lookupTransform( "/mocap_world", "/object_1/base_link", rospy.Time(0))
except:
rospy.loginfo("No <<object2>> recieved")
#continue
x_attractor = np.array([0,0,0]) + np.array(self.pos_attr) # Transform to lwr_7_link
obs_roboFrame = copy.deepcopy(self.obs) # TODO -- change, because only reference is created not copy...
for n in range(len(self.obs)): # for all bostacles
try: # Get transformation
self.pos_obs[n], self.quat_obs[n] = self.listener.lookupTransform("/mocap_world", "/object_{}/base_link".format(n), rospy.Time(0))
except:# (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("No <<object1>> recieved")
#continue
quat_thr = tf.transformations.quaternion_from_euler(self.obs[n].th_r[0],
self.obs[n].th_r[1],
self.obs[n].th_r[2])
quat_thr_mocap = tf.transformations.quaternion_multiply(self.quat_ob1, quat_thr)
th_r_mocap = tf.transformations.euler_from_quaternion(quat_thr_mocap)
# TODO apply transofrm to x0
#q_x0 = tf.transformations.quaternion_multiply( self.quat_ob1, np.hstack(( self.obs[n].x0, 0)))
#q_x0 = tf.transformations.quaternion_multiply( np.hstack((self.obs[n].x0, 0)), self.quat_ob1)
#obs_roboFrame[n].x0 = q_x0[0:3] + np.array(self.pos_ob1) # Transpose into reference frame
obs_roboFrame[n].x0 = self.ob[n].x0 + np.array(self.pos_ob[n]) # Transpose into reference frame
# TODOOOOOOOOOOOOO --- activate
obs_roboFrame[n].th_r = [th_r_mocap[0],
th_r_mocap[1],
th_r_mocap[2]]# Rotate into reference frame
pose_obs = PoseStamped() # Publish pose for verification
pose_obs.header.stamp = rospy.Time.now()
pose_obs.header.frame_id = '/mocap_world'
pose_obs.pose.orientation = Quaternion(quat_thr_mocap[0],
quat_thr_mocap[1],
quat_thr_mocap[2],
quat_thr_mocap[3])
pose_obs.pose.position = Point(obs_roboFrame[n].x0[0],
obs_roboFrame[n].x0[1],
obs_roboFrame[n].x0[2])
pub_pos1.publish(pose_obs)
traj = Path()
traj.header.stamp = rospy.Time.now()
traj.header.frame_id = '/mocap_world'
q0 = Quaternion(1, 0, 0, 0) # Unit quaternion for trajectory
x = x0_lwr7 # x for trajectory creating
x_hat = x0_lwr7 # x fro linear DS
# print('WARNING -- ERROR in obs[n].w calculation')
# print(obs_roboFrame[0])
obs_roboFrame[n].center_dyn = obs_roboFrame[n].x0
obs_roboFrame[0].w = [0,0,0]
obs_roboFrame[0].xd = [0,0,0]
#obs_roboFrame = []
loopCount = 0
for iSim in range(self.n_intSteps):
ds_init = linearAttractor_const(x, x0=x_attractor)
ds_modulated = obs_avoidance_convergence(x, ds_init, obs_roboFrame)
x = ds_modulated*self.dt + x
x_hat = ds_init*self.dt + x_hat
#x=x_hat
# Add to trajectory
pose = PoseStamped()
pose.header = traj.header
pose.pose.position = Point(x[0],x[1],x[2])
#pose.pose.position = Point(x_hat[0],x_hat[1],x_hat[2])
pose.pose.orientation = q0
traj.poses.append(pose)
loopCount += 1
pub_traj.publish(traj)
rospy.loginfo("Publishing Trajectory %s" % rospy.get_time())
self.iSim += 1
rate.sleep()
def controlLoopCB(self, event):
'''Control loop for car MPC'''
if self.goal_received and not self.goal_reached:
control_loop_start_time = time.time()
# Update system states: X=[x, y, psi]
px = self.current_pos_x
py = self.current_pos_y
car_pos = np.array([self.current_pos_x, self.current_pos_y])
psi = self.current_yaw
# Update system inputs: U=[speed(v), steering]
v = self.current_vel_odom
steering = self.steering_angle # radian
L = self.mpc.L
current_s, near_idx = self.find_current_arc_length(car_pos)
if self.DELAY_MODE:
dt_lag=self.LAG_TIME
px = px+ v*np.cos(psi)*dt_lag
py = py+ v*np.sin(psi)*dt_lag
psi= psi+ (v/L)*tan(steering)*dt_lag
current_s = current_s+self.projected_vel*dt_lag
current_state = np.array([px, py, psi, current_s])
centerPose = PoseStamped()
centerPose.header = self.create_header('map')
centerPose.pose.position.x = float(self.center_lane[near_idx, 0])
centerPose.pose.position.y = float(self.center_lane[near_idx, 1])
centerPose.pose.orientation = self.heading(self.center_point_angles[near_idx])
self.center_tangent_pub.publish(centerPose)
# Solve MPC Problem
mpc_time = time.time()
first_control, trajectory, control_input_soln = self.mpc.solve(current_state)
#use
mpc_compute_time = time.time() - mpc_time
print("Control loop time mpc=:", mpc_compute_time)
# MPC result (all described in car frame)
speed = float(first_control[0]) # speed
steering = float(first_control[1]) # radian
self.projected_vel = float(first_control[2])
if not self.mpc.WARM_START:
speed, steering = 0, 0
self.mpc.WARM_START= True
if (speed >= self.param['v_max']):
speed = self.param['v_max']
elif (speed <= (- self.param['v_max'] / 2.0)):
speed = - self.param['v_max'] / 2.0
# Display the MPC predicted trajectory
mpc_traj = Path()
mpc_traj.header = self.create_header('map')
mpc_traj.poses = []
for i in range(trajectory.shape[0]):
tempPose = PoseStamped()
tempPose.header = mpc_traj.header
tempPose.pose.position.x = trajectory[i, 0]
tempPose.pose.position.y = trajectory[i, 1]
tempPose.pose.orientation = self.heading(trajectory[i, 2])
mpc_traj.poses.append(tempPose)
print("Control loop time4=:", time.time() - control_loop_start_time)
# publish the mpc trajectory
self.mpc_trajectory_pub.publish(mpc_traj)
total_time = time.time() - control_loop_start_time
if self.DEBUG_MODE:
print("DEBUG")
print("psi: ", psi)
print("V: ", v)
print("coeffs: ", self.mpc.coeffs)
print("_steering:", steering)
print("_speed: ", speed)
print("Control loop time=:", total_time)
self.current_time += 1.0 / self.CONTROLLER_FREQ
# self.cte_plot.append(cte)
self.t_plot.append(self.current_time)
self.v_plot.append(speed)
self.steering_plot.append(np.rad2deg(steering))
self.time_plot.append(mpc_compute_time * 1000)
else:
steering = 0.0
speed = 0.0
# publish cmd
ackermann_cmd = AckermannDriveStamped()
ackermann_cmd.header = self.create_header(self.car_frame)
ackermann_cmd.drive.steering_angle = steering
self.steering_angle=steering
ackermann_cmd.drive.speed = speed
# ackermann_cmd.drive.acceleration = throttle
self.ackermann_pub.publish(ackermann_cmd)
