def publishPosition(self, position):
msg = PointStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame
msg.point = Point(position[0], position[1], 0)
self.publisher.publish(msg)
def to_msg_vector(vector):
msg = PointStamped()
msg.header = vector.header
msg.point.x = vector[0]
msg.point.y = vector[1]
msg.point.z = vector[2]
return msg
def main():
rospy.init_node('pr2_state_machine')
brain = PR2RobotBrain()
brain.getReady()
area_to_explore = PolygonStamped()
center_point = PointStamped()
now = rospy.Time.now()
area_to_explore.header.stamp = now
area_to_explore.header.frame_id = "map"
points = [Point32(-1.65, -1.56, 0.0),
Point32(5.41, -1.7, 0.0),
Point32(5.57, 4.44, 0.0),
Point32(-1.75, 4.37, 0.0)]
area_to_explore.polygon = Polygon(points)
center_point.header = area_to_explore.header
center_point.point.x = 1.83
center_point.point.y = 1.57
center_point.point.z = 0.0
brain = PR2RobotBrain(area_to_explore, center_point)
brain.loop()
def convertStereo(self, u, v, disparity):
"""
Converts two pixel coordinates u and v along with the disparity to give PointStamped
This code was taken from stereo_click package
"""
if not self.canProcess():
return None
for key in self.locks.keys():
self.locks[key].acquire()
stereoModel = image_geometry.StereoCameraModel()
stereoModel.fromCameraInfo(self.leftInfo, self.rightInfo)
(x,y,z) = stereoModel.projectPixelTo3d((u,v), disparity)
cameraPoint = PointStamped()
cameraPoint.header.frame_id = self.leftInfo.header.frame_id
cameraPoint.header.stamp = rospy.Time.now()
cameraPoint.point = Point(x,y,z)
self.listener.waitForTransform(self.outputFrame, cameraPoint.header.frame_id, rospy.Time.now(), rospy.Duration(4.0))
outputPoint = self.listener.transformPoint(self.outputFrame, cameraPoint)
for key in self.locks.keys():
self.locks[key].release()
return outputPoint
def runFilter(self):
r = rospy.Rate(self.filterRate)
while not rospy.is_shutdown():
if self.flag_reset:
self.kf.reset(self.getReset())
self.flag_reset = 0
self.kf.iteration(self.getMeasures())
self.pose_pub_.publish(self.kf.getState())
person_point = PointStamped()
person_point.header = self.kf.getState().header
person_point.header.stamp = rospy.Time.now()
person_point.point = self.kf.getState().pose.position
self.point_pub_.publish(person_point)
self.tf_person.sendTransform((self.kf.getState().pose.position.x,self.kf.getState().pose.position.y,0),
(self.kf.getState().pose.orientation.x,self.kf.getState().pose.orientation.y,self.kf.getState().pose.orientation.z,self.kf.getState().pose.orientation.w),
rospy.Time.now(),
"person_link",
self.kf.getState().header.frame_id)
r.sleep()
def move(self, group, target="", pose=None):
assert target != "" or pose is not None
# group.set_workspace([minX, minY, minZ, maxX, maxY, maxZ])
if pose is not None:
group.set_pose_target(pose)
pt = PointStamped()
pt.header = pose.header
pt.point = pose.pose.position
self.look_at_cb(pt)
else:
group.set_named_target(target)
group.allow_looking(False)
group.allow_replanning(False)
group.set_num_planning_attempts(1)
cnt = 3
while cnt > 0:
if group.go(wait=True):
return True
rospy.sleep(1)
return False
def pose_cb(self, pose):
m_pose = PointStamped()
m_pose.header = pose.header
m_pose.point = pose.pose.position
m_pose = self.listener.transformPoint(self.reference_frame,m_pose)
self.goal_x = m_pose.point.x
self.goal_y = m_pose.point.y
print "New goal: %.2f %.2f" % (self.goal_x, self.goal_y)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.5;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=-1.0
self.marker_pub.publish(marker)
def estimation(msg):
uwbdis = msg.distance
point = PointStamped()
point.header = Header()
point.header.frame_id = "vicon"
point.header.stamp = rospy.Time.now()
point.point = msg.position
listener.waitForTransform("ned", "vicon", rospy.Time.now(), rospy.Duration(0.05))
point_tf = listener.transformPoint("ned", point)
uwbanchor = array([point_tf.point.x, point_tf.point.y, point_tf.point.z])
imp = array(msg.point)
global q,a,r,xe
#xe, _ = uwb.locate(xe, Q, 1.0/100, uwbdis, uwbanchor, q, a, r)
xe, _ = vision.locate(xe, Q, 1.0/100, imp, visionanchor, q, a, r)
x_msg = state()
x_msg.state.header = Header()
x_msg.state.header.frame_id = "ned"
x_msg.state.header.stamp = rospy.Time.now()
x_msg.state.pose.position = Point(xe[0], xe[1], xe[2])
x_msg.state.pose.orientation = Quaternion(xe[3], xe[4], xe[5], xe[6])
x_msg.velocity = Vector3(xe[7], xe[8], xe[9])
x_msg.bias = xe[10]
pub.publish(x_msg)
def new_place_position(self):
current_pose = utils.manipulation.get_arm_move_group().get_current_pose()
destination = PointStamped()
destination.header.frame_id = current_pose.header.frame_id
destination.point = current_pose.pose.position
destination.point.z = 0
return destination
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 create_point_stamped(point, frame_id = 'base_link'):
m = PointStamped()
m.header.frame_id = frame_id
m.header.stamp = rospy.Time()
#m.header.stamp = rospy.get_rostime()
m.point = Point(*point)
return m
def pre_load():
rospy.loginfo('检测到预注册的位置')
rospy.loginfo('读取预设位置')
count=getpass.getuser()
read=open('/home/%s/xu_slam/src/nav_staff/map/pre_regist_pose.txt'%count,'r')
pose=read.readlines()
read.close()
poses=eval(pose[0])
try:
intial=rospy.wait_for_message("odom",Odometry)
intial_point=PointStamped()
intial_point.point=intial.pose.pose.position
intial_point.header.stamp=rospy.Time.now()
intial_point.header.frame_id='map'
except:
pass
pose_list=[]
for i in range(len(poses)):
default_point=PointStamped()
default_point.header.frame_id='map'
default_point.header.stamp=rospy.Time.now()
default_point.point.x=poses['pose_%s'%i]['x']
default_point.point.y=poses['pose_%s'%i]['y']
default_point.point.z=poses['pose_%s'%i]['z']
default_point.header.seq=i+1
pose_list.append(default_point)
pose_list.append(intial_point)
tasks(len(pose_list),pose_list)
def euroc_object_to_odom_combined(euroc_object):
header = Header(0, rospy.Time(0), euroc_object.frame_id)
# Convert the centroid
camera_point = PointStamped()
camera_point.header = header
camera_point.point = euroc_object.c_centroid
odom_point = manipulation.transform_to(camera_point, '/odom_combined')
euroc_object.c_centroid = odom_point.point
euroc_object.frame_id = '/odom_combined'
# Convert the cuboid
if euroc_object.c_cuboid_success:
cuboid_posestamped = PoseStamped(header, euroc_object.object.primitive_poses[0])
cuboid_posestamped = manipulation.transform_to(cuboid_posestamped, '/odom_combined')
euroc_object.object.primitive_poses[0] = cuboid_posestamped.pose
euroc_object.object.header.frame_id = '/odom_combined'
# Convert the mpe_object
if euroc_object.mpe_success:
for i in range(0, len(euroc_object.mpe_object.primitive_poses)):
posestamped = PoseStamped(header, euroc_object.mpe_object.primitive_poses[i])
posestamped = manipulation.transform_to(posestamped, '/odom_combined')
euroc_object.mpe_object.primitive_poses[i] = posestamped.pose
euroc_object.mpe_object.header.frame_id = '/odom_combined'
def people_cb(meas):
global mouth_center
ps = PointStamped()
ps.point = meas.pos
ps.header = meas.header
ps.header.stamp = rospy.Time(0)
point = tfl.transformPoint(model.tf_frame, ps)
mouth_center = model.project3dToPixel((point.point.x, point.point.y + mouth_offset, point.point.z))
def publish(self, msg):
pt = PointStamped()
pt.header = msg.header
pt.point = msg.pos
self._point_pub.publish(pt)
def do_transform_point(point, transform):
p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x, point.point.y, point.point.z)
res = PointStamped()
res.point.x = p[0]
res.point.y = p[1]
res.point.z = p[2]
res.header = transform.header
return res
def point_cb(self, msg):
#self.point = msg
cloud = read_points_np(msg)
point = PointStamped()
point.header = msg.header
if cloud.shape[1] == 0: return
point.point.x, point.point.y, point.point.z = cloud[0][0]
self.point = point
def ConvertToWorldPoint(self, point):
point_stamped = PointStamped();
point_stamped.header.frame_id = self.tf_name
point_stamped.point = point
point_world = self.tf_listener.transformPoint("/map", point_stamped)
return point_world.point
def transform_pos(self, pt, hdr):
ps = PointStamped()
ps.point = pt
ps.header = hdr
#ps.header.stamp = hdr.stamp
print "set:"+str(ps)
self.tfl.waitForTransform(ps.header.frame_id, '/map', ps.header.stamp, rospy.Duration(3.0))
point_in_map = self.tfl.transformPoint('/map', ps)
return point_in_map
def handle_detection(self,detection):
if self.done: return
for i in range(len(detection.ids)):
pt = PointStamped()
pt.header = detection.header
pt.point.x = detection.xs[i]
pt.point.y = detection.ys[i]
pt.point.z = detection.zs[i] + detection.hs[i]
self.cylinders[detection.ids[i]].append((pt,detection.rs[i], detection.hs[i]))
self.num_detections += 1
def callback(data):
#height = data.differential_height
height = data.height
rospy.loginfo(rospy.get_caller_id() + "Pressure: %f", height)
pt = PointStamped()
pt.header = data.header
pt.point.z = height
pub = rospy.Publisher('pressure_height_point', PointStamped, queue_size=1)
pub.publish(pt)
def publish_lla(self):
'''
Publish a point message with the current LLA computed from an odom and absodom message.
'''
_, _, lla = self.enu(self.enu_pos)
lla_msg = PointStamped()
lla_msg.header.frame_id = 'lla'
lla_msg.header.stamp = self.stamp
lla_msg.point = numpy_to_point(lla)
self.lla_pub.publish(lla_msg)
def transformPoint(self,target_frame,ps, time): r = PointStamped() self.tl.waitForTransform(target_frame,ps.header.frame_id,time, rospy.Duration(5)) point_translation_upper,point_rotation_upper = self.tl.lookupTransform(target_frame,ps.header.frame_id,time) transform_matrix = numpy.dot(tf.transformations.translation_matrix(point_translation_upper), tf.transformations.quaternion_matrix(point_rotation_upper)) xyz = tuple(numpy.dot(transform_matrix, numpy.array([ps.point.x, ps.point.y, ps.point.z, 1.0])))[:3] r.header.stamp = ps.header.stamp r.header.frame_id = target_frame r.point = geometry_msgs.msg.Point(*xyz) return r
def poseStampedToPointStamped(poseStamped):
"""
Converts PoseStamped to PointStamped
"""
pointStamped = PointStamped()
pointStamped.header = poseStamped.header
pointStamped.point.x = poseStamped.pose.position.x
pointStamped.point.y = poseStamped.pose.position.y
pointStamped.point.z = poseStamped.pose.position.z
return pointStamped
def transformToLocal(point):
pointIn = PointStamped()
pointIn.point = point
pointIn.header.frame_id = 'odom'
pointIn.header.stamp = rospy.Time(0)
pointOut = PointStamped()
pointOut = transform.transformPoint('map', pointIn)
return pointOut.point
def location_from_cluster(cluster):
pt = cloud2np(cluster).mean(axis=0)
pt_stamped = PointStamped()
pt_stamped.header = cluster.header
pt_stamped.point.x, pt_stamped.point.y, pt_stamped.point.z = pt
try:
ptt = transform_point_stamped("/table", pt_stamped)
return np.array([ptt.point.x, ptt.point.y, 0])
except tf.Exception as e:
print "exception..."
return np.array([np.nan] * 3)
def move_to_point(self, pose):
pt = PointStamped()
pt.header = pose.header
pt.point = pose.pose.position
self.look_at_pub.publish(pt)
self.group.set_pose_target(pose)
self.move_to_pose_pub.publish(pose)
if not self.group.go(wait=True):
return False
return True
def ar_cb(self, markers):
for m in markers.markers:
self.listener.waitForTransform(self.target_frame,'/RotMarker%02d'% m.id, m.header.stamp, rospy.Duration(1.0))
self.listener.waitForTransform("/%s/ground"%self.name,m.header.frame_id, m.header.stamp, rospy.Duration(1.0))
((x,y,z),rot) = self.listener.lookupTransform(self.target_frame,'/RotMarker%02d'% m.id, m.header.stamp)
L = vstack([x,y])
m_pose = PointStamped()
m_pose.header = m.header
m_pose.point = m.pose.pose.position
m_pose = self.listener.transformPoint("/%s/ground"%self.name,m_pose)
Z = vstack([m_pose.point.x,m_pose.point.y])
# TODO
self.filter.update_ar(Z,L,self.ar_precision)
def calculate_entropy(self, cov):
cov = np.array(cov)*10**10
H = PointStamped()
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'odom'
H.header = h
n = float(cov.shape[0])
inner = 2*math.pi*math.e
det = np.linalg.det(cov)
H.point.x = np.log(np.power(inner**n*det,0.5))
#print(n,np.power(inner**n*det,0.5))
return H
def convertStereo(u, v, disparity, info):
"""
Converts two pixel coordinates u and v along with the disparity to give PointStamped
"""
stereoModel = image_geometry.StereoCameraModel()
stereoModel.fromCameraInfo(info["l"], info["r"])
(x, y, z) = stereoModel.projectPixelTo3d((u, v), disparity)
cameraPoint = PointStamped()
cameraPoint.header.frame_id = info["l"].header.frame_id
cameraPoint.header.stamp = rospy.Time.now()
cameraPoint.point = Point(x, y, z)
return cameraPoint
def get_goal(self):
g = PointStamped()
g.header.frame_id = 'odom'
g.point.x = self.goal[0]
g.point.y = self.goal[1]
return g
def execute(self, userdata):
global CURRENT_STATE, TAGS_FOUND, TAG_POSE, MIDDLE_POSE, CURRENT_POSE, boxToTheLeft, MIDDLE_GOAL, BOX_MARK
CURRENT_STATE = "turn"
self.count = 0
self.marker_detected = False
while not self.marker_detected:
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = -0.4
self.cmd_pub.publish(msg)
self.rate.sleep()
userdata.current_marker = TAGS_FOUND[-1]
if self.mode == 1:
MIDDLE_POSE = TAG_POSE
BOX_MARK = TAGS_FOUND[-1]
yaw = euler_from_quaternion([
CURRENT_POSE.orientation.x, CURRENT_POSE.orientation.y,
CURRENT_POSE.orientation.z, CURRENT_POSE.orientation.w
])[2]
if -180 < yaw < 0:
boxToTheLeft = True
elif 0 < yaw < 180:
boxToTheLeft = False
if boxToTheLeft:
delta_x = -2
else:
delta_x = 2
middle_point = PointStamped()
middle_point.header.frame_id = "ar_marker_" + \
str(TAGS_FOUND[-1])
middle_point.header.stamp = rospy.Time(0)
middle_point.point.x = delta_x
self.listener.waitForTransform("odom",
middle_point.header.frame_id,
rospy.Time(0), rospy.Duration(4))
middle_point_transformed = self.listener.transformPoint(
"odom", middle_point)
quaternion = quaternion_from_euler(0, 0, -math.pi / 2)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.pose.position.x = middle_point_transformed.point.x
goal.target_pose.pose.position.y = middle_point_transformed.point.y
goal.target_pose.pose.position.z = middle_point_transformed.point.z
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
MIDDLE_GOAL = goal
self.marker_detected = False
# CURRENT_STATE = None
self.cmd_pub.publish(Twist())
return "find"
def execute(self, userdata):
global CURRENT_POSE
global CURRENT_STATE, START_POSE, END_GOAL, MARKER_POSE, PARK_MARK, SIDE_GOAL, BACK_GOAL
CURRENT_STATE = "move_closer"
self.tag_pose_base = None
self.current_marker = userdata.current_marker
max_angular_speed = 0.8
min_angular_speed = 0.0
max_linear_speed = 0.8
min_linear_speed = 0.0
while True:
if self.tag_pose_base is not None and self.tag_pose_base.position.x < self.how_close:
break
elif self.tag_pose_base is not None and self.tag_pose_base.position.x > self.how_close:
move_cmd = Twist()
if self.tag_pose_base.position.x > self.how_close + 0.1: # goal too far
move_cmd.linear.x += 0.1
elif self.tag_pose_base.position.x > self.how_close: # goal too close
move_cmd.linear.x -= 0.1
else:
move_cmd.linear.x = 0
if self.tag_pose_base.position.y < 1e-3: # goal to the left
move_cmd.angular.z -= 0.1
elif self.tag_pose_base.position.y > -1e-3: # goal to the right
move_cmd.angular.z += 0.1
else:
move_cmd.angular.z = 0
move_cmd.linear.x = math.copysign(
max(min_linear_speed,
min(abs(move_cmd.linear.x), max_linear_speed)),
move_cmd.linear.x)
move_cmd.angular.z = math.copysign(
max(min_angular_speed,
min(abs(move_cmd.angular.z), max_angular_speed)),
move_cmd.angular.z)
move_cmd.linear.x = abs(move_cmd.linear.x)
self.vel_pub.publish(move_cmd)
self.rate.sleep()
print("marker!!!:", self.current_marker)
pose = PointStamped()
pose.header.frame_id = "ar_marker_" + str(self.current_marker)
pose.header.stamp = rospy.Time(0)
pose.point.z = -2
yaw = euler_from_quaternion([
CURRENT_POSE.orientation.x, CURRENT_POSE.orientation.y,
CURRENT_POSE.orientation.z, CURRENT_POSE.orientation.w
])[2]
if boxToTheLeft:
delta_x = -2
else:
delta_x = 2
if -180 < yaw < 0:
boxToTheLeft = True
elif 0 < yaw < 180:
boxToTheLeft = False
side_pose = PointStamped()
side_pose.header.frame_id = "ar_marker_" + str(self.current_marker)
side_pose.header.stamp = rospy.Time(0)
side_pose.point.z = -0.3
side_pose.point.x = delta_x
self.listener.waitForTransform("odom", pose.header.frame_id,
rospy.Time(0), rospy.Duration(4))
pose_transformed = self.listener.transformPoint("odom", pose)
side_pose_transformed = self.listener.transformPoint("odom", side_pose)
if self.ACAP:
quaternion = quaternion_from_euler(0, 0, math.pi)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.pose.position.x = pose_transformed.point.x
goal.target_pose.pose.position.y = pose_transformed.point.y
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
BACK_GOAL = goal
side_goal = MoveBaseGoal()
side_goal.target_pose.header.frame_id = "odom"
side_goal.target_pose.pose.position.x = side_pose_transformed.point.x
side_goal.target_pose.pose.position.y = side_pose_transformed.point.y
side_goal.target_pose.pose.orientation = START_POSE.orientation
SIDE_GOAL = side_goal
PARK_MARK = self.current_marker
else:
MARKER_POSE = self.tag_pose_base
return "close_enough"
def __init__(self):
self.point_received = False
self.c_p = PointStamped()
class ProcessReads_Friis():
# Processes a bagfile to and returns a dictionary that is ready to calculate Friis values:
# d = { 'tagid1': [ PROC_READ1, PROC_READ2, ... ],
# ...
# }
def __init__(self, yaml_config, tf_listener):
# Assume tf will work fine... we'll catch errors later.
# self.listener = tf.TransformListener()
self.listener = tf_listener
# Initialize Variables
self.d = {}
self.yaml_config = yaml_config
# Start Processing Incoming Reads
rfid_arr_topic = self.yaml_config[
'rfid_arr_topic'] # eg. '/rfid/ears_reader_arr'
self.sub = rospy.Subscriber(rfid_arr_topic, RFIDreadArr, self.rfid_cb)
def proc_read(self, rr): # rr is RFIDread.msg
# Check for antenna_frame definition
if not self.yaml_config['antennas'].has_key(rr.antenna_name):
rospy.logout(
'Antenna name %s undefined in yaml_config. Skipping' %
rr.antenna_name)
return None
rdr_frame = self.yaml_config['antennas'][
rr.antenna_name] # eg. /ear_antenna_left
rdr_ps = PointStamped()
rdr_ps.header.frame_id = rdr_frame
rdr_ps.header.stamp = rospy.Time(
0) # Will want to transform the point right now.
# Check for tag ground truth definition
if not self.yaml_config['tags'].has_key(rr.tagID):
rospy.logout('TagID %s undefined in yaml_config. Skipping' %
rr.tagID)
return None
tag_frame = self.yaml_config['tags'][rr.tagID][
'child_frame'] # eg. /datacap (these are being published in other thread)
tag_ps = PointStamped()
tag_ps.header.frame_id = tag_frame
tag_ps.header.stamp = rospy.Time(
0) # Will want to transform the point right now.
try:
tag_in_rdr = self.listener.transformPoint(rdr_frame, tag_ps)
rdr_in_tag = self.listener.transformPoint(tag_frame, rdr_ps)
except Exception, e:
rospy.logout('Transform(s) failed: %s. Skipping' % e.__str__())
return None
r_rdr, theta_rdr, phi_rdr = friis.CartToSphere(tag_in_rdr.point.x,
tag_in_rdr.point.y,
tag_in_rdr.point.z)
r_tag, theta_tag, phi_tag = friis.CartToSphere(rdr_in_tag.point.x,
rdr_in_tag.point.y,
rdr_in_tag.point.z)
tag_pos = PoseStamped()
tag_pos.header.frame_id = tag_frame
tag_pos.header.stamp = rospy.Time(
0) # Will want to transform the point right now.
tag_pos.pose.orientation.w = 1.0
rdr_pos = PoseStamped()
rdr_pos.header.frame_id = rdr_frame
rdr_pos.header.stamp = rospy.Time(
0) # Will want to transform the point right now.
rdr_pos.pose.orientation.w = 1.0
rot_pos = PoseStamped() # Note this is POSE stamped!
rot_frame = self.yaml_config['rotframes'][
rr.antenna_name] # eg. /ear_pan_left
rot_pos.header.frame_id = rot_frame
rot_pos.header.stamp = rospy.Time(0)
rot_pos.pose.orientation.w = 1.0
base_pos = PoseStamped() # Note this is POSE stamped!
base_pos.header.frame_id = '/base_link'
base_pos.header.stamp = rospy.Time(0)
base_pos.pose.orientation.w = 1.0
static_rot_frame = self.yaml_config['staticrotframes'][
rr.antenna_name] # eg. /ear_pan_left
rdr_p = PointStamped()
rdr_p.header.frame_id = rdr_frame
rdr_p.header.stamp = rospy.Time(
0) # Will want to transform the point right now.
rdr_p.point.x = 1.0
tag_p = PointStamped()
tag_p.header.frame_id = tag_frame
tag_p.header.stamp = rospy.Time(
0) # Will want to transform the point right now.
try:
tag_map = self.listener.transformPose('/map', tag_pos)
rdr_map = self.listener.transformPose('/map', rdr_pos)
rot_map = self.listener.transformPose('/map', rot_pos)
base_map = self.listener.transformPose('/map', base_pos)
rdr_p_rot = self.listener.transformPoint(static_rot_frame, rdr_p)
tag_p_rot = self.listener.transformPoint(static_rot_frame, tag_p)
except Exception, e:
rospy.logout('Transform(s) failed (#2): %s. Skipping' %
e.__str__())
return None
def callback(self, data):
rospy.loginfo("Recognized objects %d", data.objects.__len__())
table_corners = []
# obtain table_offset and table_pose
to = rospy.wait_for_message(self.table_topic, MarkerArray)
# obtain Table corners ...
rospy.loginfo("Tables hull size %d", to.markers.__len__())
if not to.markers:
rospy.loginfo("No tables detected")
return
else:
# NB - D says that ORK has this set to filter based on height.
# IIRC, it's 0.6-0.8m above whatever frame is set as the floor
point_array_size = 4 # for the 4 corners of the bounding box
for i in range (0, point_array_size):
p = Point()
p.x = to.markers[0].points[i].x
p.y = to.markers[0].points[i].y
p.z = to.markers[0].points[i].z
table_corners.append(p)
# this is a table pose at the edge close to the robot, in the center of x axis
table_pose = PoseStamped()
table_pose.header = to.markers[0].header
table_pose.pose = to.markers[0].pose
# Determine table dimensions
rospy.loginfo('calculating table pose bounding box in frame: %s' % table_pose.header.frame_id)
min_x = sys.float_info.max
min_y = sys.float_info.max
max_x = -sys.float_info.max
max_y = -sys.float_info.max
for i in range (table_corners.__len__()):
if table_corners[i].x > max_x:
max_x = table_corners[i].x
if table_corners[i].y > max_y:
max_y = table_corners[i].y
if table_corners[i].x < min_x:
min_x = table_corners[i].x
if table_corners[i].y < min_y:
min_y = table_corners[i].y
table_dim = Point()
# TODO: if we don't (can't!) compute the height, should we at least give it non-zero depth?
# (would also require shifting the observed centroid down by table_dim.z/2)
table_dim.z = 0.0
table_dim.x = abs(max_x - min_x)
table_dim.y = abs(max_y - min_y)
print "Dimensions: ", table_dim.x, table_dim.y
# Temporary frame used for transformations
table_link = 'table_link'
# centroid of a table in table_link frame
centroid = PoseStamped()
centroid.header.frame_id = table_link
centroid.header.stamp = table_pose.header.stamp
centroid.pose.position.x = (max_x + min_x)/2.
centroid.pose.position.y = (max_y + min_y)/2.
centroid.pose.position.z = 0.0
centroid.pose.orientation.x = 0.0
centroid.pose.orientation.y = 0.0
centroid.pose.orientation.z = 0.0
centroid.pose.orientation.w = 1.0
# generate transform from table_pose to our newly-defined table_link
tt = TransformStamped()
tt.header = table_pose.header
tt.child_frame_id = table_link
tt.transform.translation = table_pose.pose.position
tt.transform.rotation = table_pose.pose.orientation
self.tl.setTransform(tt)
self.tl.waitForTransform(table_link, table_pose.header.frame_id, table_pose.header.stamp, rospy.Duration(3.0))
if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp):
centroid_table_pose = self.tl.transformPose(table_pose.header.frame_id, centroid)
self.pose_pub.publish(centroid_table_pose)
else:
rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link)
return
# transform each object into desired frame; add to list of clusters
cluster_list = []
object_list = []
#if only clusters on the table should be extracted
if self.extract_clusters:
cluster_list = self.extract_clusters_f()
#else try to recognize objects
if self.recognize_objects:
for i in range (data.objects.__len__()):
# rospy.loginfo("Point clouds %s", data.objects[i].point_clouds.__len__())
pc = PointCloud2()
pc = data.objects[i].point_clouds[0]
arr = pointclouds.pointcloud2_to_array(pc, 1)
arr_xyz = pointclouds.get_xyz_points(arr)
arr_xyz_trans = []
for j in range (arr_xyz.__len__()):
ps = PointStamped();
ps.header.frame_id = table_link
ps.header.stamp = table_pose.header.stamp
ps.point.x = arr_xyz[j][0]
ps.point.y = arr_xyz[j][1]
ps.point.z = arr_xyz[j][2]
if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp):
ps_in_kinect_frame = self.tl.transformPoint(table_pose.header.frame_id, ps)
else:
rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link)
return
arr_xyz_trans.append([ps_in_kinect_frame.point.x, ps_in_kinect_frame.point.y, ps_in_kinect_frame.point.z])
pc_trans = PointCloud2()
pc_trans = pointclouds.xyz_array_to_pointcloud2(np.asarray([arr_xyz_trans]),
table_pose.header.stamp, table_pose.header.frame_id)
#self.pub.publish(pc_trans)
object_list.append(pc_trans)
rospy.loginfo("object size %d", object_list.__len__())
cluster_centroids = []
object_centroids = []
for cloud in range (cluster_list.__len__()):
cluster_centroids.append(self.compute_centroid(cluster_list[cloud]))
for cloud in range (object_list.__len__()):
object_centroids.append(self.compute_centroid(object_list[cloud]))
recognized_objects = []
indices = []
for centroid in range (cluster_centroids.__len__()):
# dist = self.closest(np_cluster_centroids, np.asarray(cluster_centroids[centroid]))
if object_centroids:
indices = self.do_kdtree(np.asarray(object_centroids), np.asarray(cluster_centroids[centroid]), self.search_radius)
if not indices:
recognized_objects.append(0)
else:
recognized_objects.append(1)
print "recognized objects: ", recognized_objects
# finally - save all data in the object that'll be sent in response to actionserver requests
with self.result_lock:
self._result.objects = cluster_list
self._result.recognized_objects = recognized_objects
self._result.table_dims = table_dim
self._result.table_pose = centroid_table_pose
self.has_data = True
def free_spots_from_dimensions(table, object_dims, blocking_objs, res=0.1):
'''
Currently works only with a single shape
Assumes table is in xy plane
'''
#find the lower left corner of the table in the header frame
#we want everything relative to this point
table_corners = gt.bounding_box_corners(table.shapes[0])
lower_left = Pose()
lower_left.position = gt.list_to_point(table_corners[0])
lower_left.orientation.w = 1.0
#rospy.loginfo('Table corners are:')
#for c in table_corners: rospy.loginfo('\n'+str(c))
#rospy.loginfo('Lower left =\n'+str(lower_left))
#rospy.loginfo('Table header =\n'+str(table.header))
#rospy.loginfo('Table pose =\n'+str(table.poses[0]))
#this is the position of the minimum x, minimum y point in the table's header frame
table_box_origin = gt.transform_pose(lower_left, table.poses[0])
tr = gt.inverse_transform_list(gt.transform_list(table_corners[-1], table.poses[0]), table_box_origin)
table_dims = (tr[0], tr[1])
tbos = PoseStamped()
tbos.header = table.header
tbos.pose = table_box_origin
marray.markers.append(vt.marker_at(tbos, ns='table_origin', mtype=Marker.CUBE, r=1.0))
max_box = PointStamped()
max_box.header = table.header
max_box.point = gt.list_to_point(gt.transform_list([table_dims[0], table_dims[1], 0], table_box_origin))
marray.markers.append(vt.marker_at_point(max_box, ns='table_max', mtype=Marker.CUBE, r=1.0))
#rospy.loginfo('Table box origin is '+str(table_box_origin)+' dimensions are '+str(table_dims))
locs_on_table = _get_feasible_locs(table_dims, object_dims, res)
for i, l in enumerate(locs_on_table):
pt = Point()
pt.x = l[0]
pt.y = l[1]
mpt = PointStamped()
mpt.header = table.header
mpt.point = gt.transform_point(pt, table_box_origin)
marray.markers.append(vt.marker_at_point(mpt, mid=i, ns='locations', r=1.0, g=1.0, b=0.0))
feasible_locs = []
#check that these points really are on the table
for i, l in enumerate(locs_on_table):
pt = Point()
pt.x = l[0]
pt.y = l[1]
#this point is now defined relative to the origin of the table (rather than its minimum x, minimum y point)
table_pt = gt.inverse_transform_point(gt.transform_point(pt, table_box_origin), table.poses[0])
if point_on_table(table_pt, table.shapes[0]):
feasible_locs.append(l)
marray.markers[i+2].color.r = 0.0
marray.markers[i+2].color.b = 1.0
rospy.loginfo('Testing '+str(len(feasible_locs))+' locations')
if not feasible_locs:
return feasible_locs
forbidden=[]
for i, o in enumerate(blocking_objs):
ofcs = gt.bounding_box_corners(o.shapes[0])
objpose = tl.transform_pose(table.header.frame_id, o.header.frame_id, o.poses[0])
hfcs = [gt.transform_list(c, objpose) for c in ofcs]
tfcs = [gt.inverse_transform_list(c, table_box_origin) for c in hfcs]
oxmax = max([c[0] for c in tfcs])
oxmin = min([c[0] for c in tfcs])
oymax = max([c[1] for c in tfcs])
oymin = min([c[1] for c in tfcs])
forbidden.append(((oxmin, oymin), (oxmax - oxmin, oymax - oymin)))
#rospy.loginfo('\n'+str(forbidden[-1]))
ps = PoseStamped()
ps.header = table.header
ps.pose = objpose
ps = PoseStamped()
ps.header = table.header
ps.pose.position = gt.list_to_point(gt.transform_list([oxmin, oymin, 0], table_box_origin))
ps.pose.orientation.w = 1.0
marray.markers.append(vt.marker_at(ps, ns='forbidden', mid=i, r=1.0, b=0.0))
# Remove forbidden rectangles
for (bottom_left, dims) in forbidden:
_remove_feasible_locs(feasible_locs, object_dims,
bottom_left,
_add(bottom_left, dims),
res)
rospy.loginfo('There are '+str(len(feasible_locs))+' possible locations')
obj_poses = []
for i, fl in enumerate(feasible_locs):
table_frame_pose = Pose()
table_frame_pose.position.x = fl[0] + object_dims[0]/2.0
table_frame_pose.position.y = fl[1] + object_dims[1]/2.0
table_frame_pose.orientation.w = 1.0
pose = PoseStamped()
pose.header = copy.deepcopy(table.header)
pose.pose = gt.transform_pose(table_frame_pose, table_box_origin)
obj_poses.append(pose)
pt = PointStamped()
pt.header = table.header
pt.point = pose.pose.position
marray.markers.append(vt.marker_at_point(pt, mid=i, ns='final_locations', g=1.0, b=0.0))
#rospy.loginfo('Object poses are:')
#for op in obj_poses: rospy.loginfo(str(op))
for i in range(10):
vpub.publish(marray)
rospy.sleep(0.1)
return obj_poses
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
from geometry_msgs.msg import PointStamped, Point, Pose, Quaternion, Twist, Vector3
if __name__ == '__main__':
rospy.init_node('turtle_tf_listener')
listener = tf.TransformListener()
#listener.waitForTransform('base_link', 'base_laser', rospy.Time(0),rospy.Duration(4.0))
#we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
laser_point = PointStamped()
laser_point.header.frame_id = 'base_laser'
#we'll just use the most recent transform available for our simple example
laser_point.header.stamp = rospy.Time(0)
#just an arbitrary point in space
laser_point.point.x = 3.0
laser_point.point.y = 0.0
laser_point.point.z = 0.0
now = rospy.get_rostime()
#we'll transform a point once every second
rate = rospy.Rate(1.0)
def __init__(self):
self.N = 40
self.xmax = 50.0
self.ymax = 25.0
self.lam = 0.125
self.t_max = 30
self.memory = 1
self.count = 0
self.init = False
self.collisionDetected = False
self.reachedGoal = False
self.gotObstacles = False
self.gotGoal = False
self.timeOut = False
self.first = True
self.goal_pnt = PointStamped()
self.flightevent = QuadFlightEvent()
self.flightevent.mode = self.flightevent.KILL
self.goal_pnt.point.x = 60
self.goal_pnt.point.y = 0
self.goal_pnt.point.z = 1
self.goal = np.array([self.goal_pnt.point.x, self.goal_pnt.point.y])
self.alt = 0
self.goal_alt = self.goal_pnt.point.z
self.numObstacles = 0
self.status_prev = 10
self.statusSub = rospy.Subscriber('flight_status', FloatStamped,
self.statusCB)
self.poseSub = rospy.Subscriber('/LQ02s/vicon', ViconState,
self.poseCB)
self.statusPub = rospy.Publisher("flight_status",
FloatStamped,
queue_size=1,
latch=True)
self.pubGoal = rospy.Publisher("/LQ02s/global_goal",
PointStamped,
queue_size=1)
self.pubEvent = rospy.Publisher('/LQ02s/event',
QuadFlightEvent,
queue_size=1,
latch=True)
rospy.Timer(rospy.Duration(0.1), self.goalTimer)
self.openFile()
self.start_process()
self.spawn_field()
self.start_up()
def transformPt(target_frame, pt, header):
ps = PointStamped()
ps.header = header
ps.point = pt
return listener.transformPoint(target_frame, ps).point
def __init__(self):
self.bridge = CvBridge()
self.fruitPoint = PointStamped()
rospy.Subscriber("/recolect/camera1/image_raw", Image, self.callback)
rospy.init_node('prediction')
pub_real = rospy.Publisher('real_position', PointStamped)
pub_prediction = rospy.Publisher('prediction_result', PolygonStamped)
r = rospy.Rate(100)
a = np.loadtxt(r"robo_dataset.txt")
robo1_real = a[:, [0, 1]]
robo1_simu_temp = []
for i in xrange(0, a.shape[0]):
if i == 0:
k = a[i, [0, 1]]
p = a[i, [0, 1]]
robo1_simu_temp.append([k[0], k[1]])
pub_real.publish(
PointStamped(Header(frame_id="base_link", stamp=rospy.Time()),
Point(k[0], k[1], 0)))
pub_prediction.publish(
PolygonStamped(
Header(frame_id="base_link", stamp=rospy.Time()),
Polygon([
Point32(p[0], p[1], 0),
Point32(0, 0, 0),
Point32(10, 10, 0)
])))
r.sleep()
continue
k_1 = k
k = compare(k_1, p, a[i])
p = k + k - k_1
robo1_simu_temp.append([k[0], k[1]])
pub_real.publish(
def AR_Drone(x, y, z, u, v, w, phi, theta, psi, p, q, r):
global ctrl_in, g, Ixx, Iyy, Izz, euler_max, yaw_rate_max, alt_rate_max, ctrl_size, m, w_old, T_kp
#------------#
# Inputs #
#------------#
if len(ctrl_in) == 0:
phi_c = -ctrl_in.phi * euler_max # roll
theta_c = -ctrl_in.theta * euler_max # pitch
r_c = -ctrl_in.psi * yaw_rate_max # yaw_rate
zdot_c = ctrl_in.T
else:
phi_c = -ctrl_in[0].phi * euler_max # roll
theta_c = -ctrl_in[0].theta * euler_max # pitch
r_c = -ctrl_in[0].psi * yaw_rate_max # yaw_rate
zdot_c = ctrl_in[0].T
#--------------#
# Velocity #
#--------------#
dxdt = np.asmatrix(np.zeros((12, 1)))
dxdt[0] = u
dxdt[1] = v
dxdt[2] = w
#------------------#
# Acceleration #
#------------------#
global roll_kg, pitch_kg
Z_body_acceleration = ((zdot_c - w) / .3 + g) / (cos(roll_kg * phi_c) *
cos(pitch_kg * theta_c))
body_frame_acceleration = np.matrix([[0], [0], [Z_body_acceleration]])
dxdt[3:6] = np.multiply(
(rotB2W(-phi, theta, psi) * body_frame_acceleration -
np.matrix([[0], [0], [g]])), np.matrix([[-1], [-1], [1]]))
acc = PointStamped()
acc.point.x = dxdt[3, -1]
acc.point.y = dxdt[4, -1]
acc.point.z = dxdt[5, -1]
pub_acc.publish(acc)
#----------------------#
# Angular Velocity #
#----------------------#
# Gyro to Body Rotation
G2B = np.matrix([[1, sin(phi)*tan(theta), cos(phi)*tan(theta)],\
[0, cos(phi), -sin(phi)],\
[0, sin(phi)/cos(theta), cos(phi)/cos(theta)]])
dxdt[6:9] = G2B * np.matrix([[p], [q], [r]])
#--------------------------#
# Angular Acceleration #
#--------------------------#
global roll_kp, roll_kd, pitch_kp, pitch_kd, yaw_kp, yaw_ki
tauPhi = pidControl(roll_kp, roll_kd, roll_kg, phi, phi_c, p)
tauTheta = pidControl(pitch_kp, pitch_kd, pitch_kg, theta, theta_c, q)
tauPsi = pidControl(yaw_kp, 0, yaw_kg, r, r_c, 0)
dxdt[9] = (q * r * (Iyy - Izz) / Ixx) + tauPhi / Ixx
dxdt[10] = (p * r * (Izz - Ixx) / Iyy) + tauTheta / Iyy
dxdt[11] = (p * q * (Ixx - Iyy) / Izz) + tauPsi / Izz
return dxdt
def process_feedback(self, feedback):
if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
# feedback.control_name == "spawn_menu":
if feedback.menu_entry_id == 1:
self.count += 1
# rospy.loginfo(feedback)
body = Body()
body.name = "imarker_spawned_body_" + str(self.count)
# TODO(lucasw)
body.mass = 1.0
try:
trans = self.tf_buffer.lookup_transform("map", self.spawn_frame,
rospy.Time())
except (tf2_ros.LookupException,
tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logerr("tf2_ros exception")
rospy.logerr(e)
return
body.pose.position.x = trans.transform.translation.x
body.pose.position.y = trans.transform.translation.y
body.pose.position.z = trans.transform.translation.z
body.pose.orientation = trans.transform.rotation
# TODO(lucasw) add twist linear with another interactive tf,
# and twist angular with a second interactive tf?
body.type = Body.BOX
body.scale.x = self.im.controls[0].markers[0].scale.x / 2.0
body.scale.y = self.im.controls[0].markers[0].scale.y / 2.0
body.scale.z = self.im.controls[0].markers[0].scale.z / 2.0
# can't get this to work
# http://answers.ros.org/question/249433/tf2_ros-buffer-transform-pointstamped/
if False:
feedback_pt = PointStamped()
feedback_pt.header = feedback.header
feedback_pt.point.x = self.linear_vel_pt[0]
feedback_pt.point.y = self.linear_vel_pt[1]
feedback_pt.point.z = self.linear_vel_pt[2]
self.tf_buffer.registration.print_me()
try:
pt_in_map = self.tf_buffer.transform(feedback_pt, "map",
rospy.Duration(2.0), PointStamped)
except tf2_ros.TypeException as e:
rospy.logerr(e)
return
rospy.loginfo(f'output {pt_in_map}')
else:
quat = [trans.transform.rotation.x,
trans.transform.rotation.y,
trans.transform.rotation.z,
trans.transform.rotation.w]
mat = tf.transformations.quaternion_matrix(quat)
pt_in_map = numpy.dot(mat, self.linear_vel_pt)
body.twist.linear.x = pt_in_map[0]
body.twist.linear.y = pt_in_map[1]
body.twist.linear.z = pt_in_map[2]
# rospy.loginfo(body.twist.linear)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = False
add_compound_request.body.append(body)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def callback_mocap(odometry_msg):
global ranges
global d_min
global i
if not len(traj_x) == 0 and not len(ranges) == 0:
x_pos = odometry_msg.pose.pose.position.x
y_pos = odometry_msg.pose.pose.position.y
yaw = odometry_msg.pose.pose.orientation.z
v = odometry_msg.twist.twist.linear.x
state_m = State(x_pos, y_pos, yaw, v)
print("stuck1")
ind = calc_target_index(state_m, traj_x, traj_y)
min_dist = min(ranges)
dist_tg = dist_target(state_m, traj_x, traj_y)
if min_dist < 0.6 and dist_tg > d_min:
angle_list = []
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = 0
ctrl_pub.publish(control_request)
for i in range(
len(ranges)
): # the program might be checking in each increment angle if there is obstacle in the zone
angle = angle_min + i * increment
angle_list.append(angle)
two_obstacles(ranges, angle_list)
if -(90 * math.pi / 180) <= angle_list[i] <= -(70 * math.pi /
180):
print("-90 to -70")
if ranges[i] < 0.2:
print("-90 to -70 and range less than 0.2")
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = (15 * math.pi / 180) * 100
ctrl_pub.publish(control_request)
if -(70 * math.pi / 180) < angle_list[i] <= -(50 * math.pi /
180):
print("-70 to -50")
if ranges[i] < 0.3:
print("-70 to -50 and range less than 0.3")
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = (25 * math.pi / 180) * 100
ctrl_pub.publish(control_request)\
if -(50 * math.pi / 180) < angle_list[i] <= -(30 * math.pi /
180):
print("-50 to -30")
if ranges[i] < 0.4:
print("-50 to -30 and range less than 0.4")
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = (35 * math.pi / 180) * 100
ctrl_pub.publish(control_request)
if -(30 * math.pi / 180) < angle_list[i] <= -(10 * math.pi /
180):
print("-30 to -10")
if ranges[i] < 0.5:
print("-30 to -10 and range less than 0.5")
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = (45 * math.pi / 180) * 100
ctrl_pub.publish(control_request)
if -(10 * math.pi / 180) < angle_list[i] <= (10 * math.pi /
180):
print("-10 to 10")
if ranges[i] < 0.6:
print("-10 to 10 and range less than 0.6")
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = -(55 * math.pi / 180) * 100
ctrl_pub.publish(control_request)
if (10 * math.pi / 180) < angle_list[i] <= (30 * math.pi /
180):
print("10 to 30")
if ranges[i] < 0.5:
print("10 to 30 and range less than 0.5")
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = -(45 * math.pi / 180) * 100
ctrl_pub.publish(control_request)
if (30 * math.pi / 180) < angle_list[i] <= (50 * math.pi /
180):
print("30 to 50")
if ranges[i] < 0.4:
print("30 to 50 and range less than 0.4")
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = -(35 * math.pi / 180) * 100
ctrl_pub.publish(control_request)
if (50 * math.pi / 180) < angle_list[i] <= (70 * math.pi /
180):
print("50 to 70")
if ranges[i] < 0.3:
print("50 to 70 and range less than 0.3")
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = -(25 * math.pi / 180) * 100
ctrl_pub.publish(control_request)
if (70 * math.pi / 180) <= angle_list[i] <= (90 * math.pi /
180):
print("70 to 90")
if ranges[i] < 0.2:
print("70 to 90 and range less than 0.2")
control_request = lli_ctrl_request()
control_request.velocity = 25
control_request.steering = -(15 * math.pi / 180) * 100
ctrl_pub.publish(control_request)
elif min_dist < 0.15:
while min_dist < 0.2:
control_request = lli_ctrl_request()
control_request.velocity = 0
control_request.steering = 0
ctrl_pub.publish(control_request)
print("emergency stop!")
#two_obstacles(ranges, angle_list)
else:
#control_request = lli_ctrl_request() # think the car stopped when it
#control_request.velocity = 20 # passed all obstacles, so this
#control_request.steering = 0 # should bump it a bit forward
#ctrl_pub.publish(control_request) # and prevent it to be stopped
if ind < len(traj_x) - 1:
print('Running Trajectory')
ind = calc_target_index(state_m, traj_x, traj_y)
print("stuck2")
delta, ind = pure_pursuit_control(state_m, traj_x, traj_y, ind)
target_pose = PointStamped()
target_pose.header.stamp = rospy.Time.now()
target_pose.header.frame_id = '/qualisys'
target_pose.point.x = traj_x[ind]
target_pose.point.y = traj_y[ind]
target_pub.publish(target_pose)
target_angle = max(-80, min(delta / (math.pi / 4) * 100, 80))
control_request = lli_ctrl_request()
control_request.velocity = target_speed
control_request.steering = target_angle
ctrl_pub.publish(control_request)
elif min_dist < 0.15:
while min_dist < 0.2:
control_request = lli_ctrl_request()
control_request.velocity = 0
control_request.steering = 0
ctrl_pub.publish(control_request)
print("emergency stop!")
else:
print("### DONE WITH TRAJECTORY")
control_request = lli_ctrl_request()
control_request.velocity = 0
control_request.steering = 0
ctrl_pub.publish(control_request)
def _tfTransformTags(self, target_frame):
""" Convert all of the visible tags to target frame.
Args:
target_frame (string): The desired final coordinate frame.
Returns:
A geometry_msgs.msg.PoseStamped dictionary containing the positions in the target frame
of the visible AprilTags that were successfully transformed.
Note:
Raw tag orientation data comes in the /ar_marker_<id> frame, and its position data comes in the
/camera_rgb_optical_frame, so our transformations must reflect this.
Also note that this is the scary function...
"""
# transform the visible tags that are in the viable field of view
transformed = {}
for id in self.tags:
# make sure that the data coming in is in a viable frame of view, and ignore if it's not
# experimentally, I found points more than 7pi/15 rad away from the x-axis gave junk data
if abs(
atan2(self.tags[id].pose.position.x,
self.tags[id].pose.position.z)) > self._AR_FOV_LIMIT:
self._logger.warn("Tag outside FOV. Ignoring.")
continue
# since the tag should always be roughly perpendicular to the ground, these values should be relatively small
if np.isclose(self.tags[id].pose.orientation.x, 1,
atol=0.01) or np.isclose(
self.tags[id].pose.orientation.y, 1, atol=0.01):
self._logger.warn(
"Tag outside acceptable orientation limits. Ignoring.")
continue
# if abs(self.tags[id].pose.orientation.x) > 0.75 or abs(self.tags[id].pose.orientation.y) > 0.75:
# continue
# get the header from the current tag
header = self.tags[id].header
# set the time to show that we only care about the most recent available transform
header.stamp = rospy.Time(0)
# orientation data is in the ar_marker_<id> frame, so we need to update the starting frame
# (if we just transform from the optical frame, then turning the AR tag upside down affects the
# reported orientation)
# this will get us the angle between the ARtag's x-axis and the robot base's x-axis
header.frame_id = '/ar_marker_' + str(id)
orientation = self._tf_listener.transformQuaternion(
target_frame,
QuaternionStamped(header, self.tags[id].pose.orientation))
# make sure the look-up succeeded
if orientation is None:
continue
# incoming position data is relative to the rgb camera frame, so we reset the header to the optical
# frame to get the correct position (note that this step is necessary since we're getting a shallow
# copy of the header)
header.frame_id = '/camera_rgb_optical_frame'
position = self._tf_listener.transformPoint(
target_frame, PointStamped(header,
self.tags[id].pose.position))
# make sure the look-up succeeded
if position is None:
continue
# if we made it this far, then we can add our pose data to our dictionary!
transformed[id] = PoseStamped(
position.header, Pose(position.point, orientation.quaternion))
return transformed
#!/usr/bin/env python
import roslib
import rospy
import geometry_msgs.msg
from nav_msgs.msg import Odometry
from std_msgs.msg import Float32
from geometry_msgs.msg import PointStamped
z = 0
w = 0
command = PointStamped()
pub = rospy.Publisher('xy_position_data', PointStamped, queue_size=10)
def orientation():
rospy.init_node("orientation")
rate = rospy.Rate(10)
rospy.Subscriber("/odom", Odometry, Callback)
rospy.spin()
def Callback(data):
global left
global right
z = float(data.pose.pose.position.x)
w = float(data.pose.pose.position.y)
rospy.loginfo("Orientation_sensor_1= %s", z)
rospy.loginfo("Orientation_sensor_2= %s", w)
def publish_tracked_people(self, now):
"""
Publish markers of tracked people to Rviz and to <people_tracked> topic
"""
people_tracked_msg = PersonArray()
people_tracked_msg.header.stamp = now
people_tracked_msg.header.frame_id = self.publish_people_frame
marker_id = 0
# Make sure we can get the required transform first:
if self.use_scan_header_stamp_for_tfs:
tf_time = now
try:
self.listener.waitForTransform(self.publish_people_frame, self.fixed_frame, tf_time, rospy.Duration(1.0))
transform_available = True
except:
transform_available = False
else:
tf_time = rospy.Time(0)
transform_available = self.listener.canTransform(self.publish_people_frame, self.fixed_frame, tf_time)
marker_id = 0
if not transform_available:
rospy.loginfo("Person tracker: tf not avaiable. Not publishing people")
else:
for person in self.objects_tracked:
if person.is_person == True:
if self.publish_occluded or person.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
# Get position in the <self.publish_people_frame> frame
ps = PointStamped()
ps.header.frame_id = self.fixed_frame
ps.header.stamp = tf_time
ps.point.x = person.pos_x
ps.point.y = person.pos_y
try:
ps = self.listener.transformPoint(self.publish_people_frame, ps)
except:
rospy.logerr("Not publishing people due to no transform from fixed_frame-->publish_people_frame")
continue
# publish to people_tracked topic
new_person = Person()
new_person.pose.position.x = ps.point.x
new_person.pose.position.y = ps.point.y
yaw = math.atan2(person.vel_y, person.vel_x)
quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
new_person.pose.orientation.x = quaternion[0]
new_person.pose.orientation.y = quaternion[1]
new_person.pose.orientation.z = quaternion[2]
new_person.pose.orientation.w = quaternion[3]
new_person.id = person.id_num
people_tracked_msg.people.append(new_person)
# publish rviz markers
# Cylinder for body
marker = Marker()
marker.header.frame_id = self.publish_people_frame
marker.header.stamp = now
marker.ns = "People_tracked"
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = (rospy.Duration(3) - (rospy.get_rostime() - person.last_seen)).to_sec()/rospy.Duration(3).to_sec() + 0.1
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.id = marker_id
marker_id += 1
marker.type = Marker.CYLINDER
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 1.2
marker.pose.position.z = 0.8
self.marker_pub.publish(marker)
# Sphere for head shape
marker.type = Marker.SPHERE
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.pose.position.z = 1.5
marker.id = marker_id
marker_id += 1
self.marker_pub.publish(marker)
# Text showing person's ID number
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.id = marker_id
marker_id += 1
marker.type = Marker.TEXT_VIEW_FACING
marker.text = str(person.id_num)
marker.scale.z = 0.2
marker.pose.position.z = 1.7
self.marker_pub.publish(marker)
# Arrow pointing in direction they're facing with magnitude proportional to speed
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = (rospy.Duration(3) - (rospy.get_rostime() - person.last_seen)).to_sec()/rospy.Duration(3).to_sec() + 0.1
start_point = Point()
end_point = Point()
start_point.x = marker.pose.position.x
start_point.y = marker.pose.position.y
end_point.x = start_point.x + 0.5*person.vel_x
end_point.y = start_point.y + 0.5*person.vel_y
marker.pose.position.x = 0.
marker.pose.position.y = 0.
marker.pose.position.z = 0.1
marker.id = marker_id
marker_id += 1
marker.type = Marker.ARROW
marker.points.append(start_point)
marker.points.append(end_point)
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0.2
self.marker_pub.publish(marker)
# <self.confidence_percentile>% confidence bounds of person's position as an ellipse:
cov = person.filtered_state_covariances[0][0] + person.var_obs # cov_xx == cov_yy == cov
std = cov**(1./2.)
gate_dist_euclid = scipy.stats.norm.ppf(1.0 - (1.0-self.confidence_percentile)/2., 0, std)
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.type = Marker.SPHERE
marker.scale.x = 2*gate_dist_euclid
marker.scale.y = 2*gate_dist_euclid
marker.scale.z = 0.01
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = 0.1
marker.pose.position.z = 0.0
marker.id = marker_id
marker_id += 1
self.marker_pub.publish(marker)
# Clear previously published people markers
for m_id in xrange(marker_id, self.prev_person_marker_id):
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = self.publish_people_frame
marker.ns = "People_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_person_marker_id = marker_id
# Publish people tracked message
self.people_tracked_pub.publish(people_tracked_msg)
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PointStamped
from std_msgs.msg import Header
from geometry_msgs.msg import Point
if __name__ == '__main__':
rospy.init_node('goal_point_publisher', anonymous=True)
publisher = rospy.Publisher('/goal_point', PointStamped, queue_size=1)
while True:
header = Header(stamp=rospy.Time.now(), frame_id="laser")
point = Point(1, 1, 1)
point_stamped = PointStamped(header=header, point=point)
publisher.publish(point_stamped)
def test_open_drawer(
self, kitchen_setup): # where is the kitchen_setup actually loaded
""""
:type kitchen_setup: Boxy
"""
handle_frame_id = u'iai_kitchen/sink_area_left_middle_drawer_handle'
handle_name = u'sink_area_left_middle_drawer_handle'
bar_axis = Vector3Stamped()
bar_axis.header.frame_id = handle_frame_id
bar_axis.vector.y = 1
bar_center = PointStamped()
bar_center.header.frame_id = handle_frame_id
tip_grasp_axis = Vector3Stamped()
tip_grasp_axis.header.frame_id = kitchen_setup.l_tip
tip_grasp_axis.vector.y = 1
kitchen_setup.add_json_goal(u'GraspBar',
root=kitchen_setup.default_root,
tip=kitchen_setup.l_tip,
tip_grasp_axis=tip_grasp_axis,
bar_center=bar_center,
bar_axis=bar_axis,
bar_length=0.4)
# Create gripper from kitchen object
x_gripper = Vector3Stamped()
x_gripper.header.frame_id = kitchen_setup.l_tip
x_gripper.vector.z = 1
# Get goal for grasping the handle
x_goal = Vector3Stamped()
x_goal.header.frame_id = handle_frame_id
x_goal.vector.x = -1
# Align planes for gripper to be horizontal/vertical
kitchen_setup.align_planes(kitchen_setup.l_tip,
x_gripper,
root_normal=x_goal)
kitchen_setup.allow_all_collisions() # makes execution faster
kitchen_setup.send_and_check_goal() # send goal to Giskard
kitchen_setup.add_json_goal(u'Open',
tip=kitchen_setup.l_tip,
object_name=u'kitchen',
handle_link=handle_name)
kitchen_setup.allow_all_collisions() # makes execution faster
kitchen_setup.send_and_check_goal() # send goal to Giskard
# Update kitchen object
kitchen_setup.set_kitchen_js(
{u'sink_area_left_middle_drawer_main_joint': 0.48})
# Close drawer partially
kitchen_setup.add_json_goal(u'OpenDrawer',
tip=kitchen_setup.l_tip,
object_name=u'kitchen',
handle_link=handle_name,
distance_goal=0.2)
kitchen_setup.allow_all_collisions() # makes execution faster
kitchen_setup.send_and_check_goal() # send goal to Giskard
# Update kitchen object
kitchen_setup.set_kitchen_js(
{u'sink_area_left_middle_drawer_main_joint': 0.2})
kitchen_setup.add_json_goal(u'Close',
tip=kitchen_setup.l_tip,
object_name=u'kitchen',
handle_link=handle_name)
kitchen_setup.allow_all_collisions() # makes execution faster
kitchen_setup.send_and_check_goal() # send goal to Giskard
# Update kitchen object
kitchen_setup.set_kitchen_js(
{u'sink_area_left_middle_drawer_main_joint': 0.0})
pass
def pre_regist(odom, modle):
if odom:
intial = rospy.wait_for_message("odom", Odometry)
intial_point = PointStamped()
intial_point.point = intial.pose.pose.position
intial_point.header.stamp = rospy.Time.now()
intial_point.header.frame_id = 'map'
else:
pass
pose_list, pose_dic, poses = [], {}, {}
if modle == 'cruise':
for i in range(3): #default 3
rospy.loginfo('请在地图上用 publish point 输入第%s个您希望机器人到达的位置' % (i + 1))
pose = rospy.wait_for_message("clicked_point", PointStamped)
pose_list.append(pose)
pose_dic = {
'pose_%s' % i: {
'x': pose.point.x,
'y': pose.point.y,
'z': pose.point.z
}
}
poses.update(pose_dic)
print 'position', 1 + i, 'recieved'
elif modle == 'voice_interface':
for i in range(10):
rospy.loginfo('请在地图上用 publish point 输入第%s个您希望机器人到达的位置' % (i + 1))
pose = rospy.wait_for_message("clicked_point", PointStamped)
pose_list.append(pose)
pose_dic = {
'pose_%s' % i: {
'x': pose.point.x,
'y': pose.point.y,
'z': pose.point.z
}
}
poses.update(pose_dic)
print 'position', 1 + i, 'recieved'
else:
rospy.loginfo('error unkown module')
# if back to initial
try:
pose_list.append(intial_point)
except:
pass
# store
count = getpass.getuser()
try:
write = open(
'/home/%s/xu_slam/src/nav_staff/map/pre_regist_pose.txt' % count,
'w')
except:
os.makedirs('/home/%s/xu_slam/src/nav_staff/map' % count)
write = open(
'/home/%s/xu_slam/src/nav_staff/map/pre_regist_pose.txt' % count,
'w')
write.writelines('%s' % poses)
write.close()
tasks(len(pose_list), pose_list)
def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05 #rospy.get_param(padd_name)
joint_names = [
'%s_%s' % ('r', j) for j in [
'shoulder_pan_joint', 'shoulder_lift_joint',
'upper_arm_roll_joint', 'elbow_flex_joint',
'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
]
]
goal = MoveArmGoal()
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.goal_constraints.joint_constraints = [
JointConstraint() for i in range(len(joint_names))
]
for i in range(len(joint_names)):
goal.motion_plan_request.goal_constraints.joint_constraints[
i].joint_name = joint_names[i]
goal.motion_plan_request.goal_constraints.joint_constraints[
i].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_above = 0.08
goal.motion_plan_request.goal_constraints.joint_constraints[
i].tolerance_below = 0.08
for z in range(2):
min_dist = 1000
if (z % 2 == 0):
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = -2.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
else:
goal.motion_plan_request.goal_constraints.joint_constraints[
0].position = 0.0
goal.motion_plan_request.goal_constraints.joint_constraints[
3].position = -0.2
goal.motion_plan_request.goal_constraints.joint_constraints[
5].position = -0.2
for x in range(3):
self.move_arm_action_client.send_goal(goal)
r = rospy.Rate(10)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state !=
actionlib_msgs.msg.GoalStatus.PENDING):
break
#getting right finger tip link in base_link frame
t = self.tf.getLatestCommonTime(
"/base_link", "/r_gripper_r_finger_tip_link")
finger_point = PointStamped()
finger_point.header.frame_id = "/r_gripper_r_finger_tip_link"
finger_point.header.stamp = t
finger_point.point.x = 0
finger_point.point.y = 0
finger_point.point.z = 0
finger_point_base = self.tf.transformPoint(
"base_link", finger_point)
distance = math.sqrt(
math.pow(finger_point_base.point.x - .6, 2) +
math.pow(finger_point_base.point.y + .6, 2))
# pole is .1 in diameter
distance -= .1
if distance < min_dist:
rospy.loginfo("X: %g Y: %g Dist: %g",
finger_point_base.point.x,
finger_point_base.point.y, distance)
min_dist = distance
r.sleep()
end_state = self.move_arm_action_client.get_state()
if (end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED):
break
rospy.loginfo("Min dist %d is %g", z, min_dist)
#should be a .5 buffer, allowing .1 buffer
self.failIf(min_dist < (allow_padd - extra_buffer))
final_state = self.move_arm_action_client.get_state()
self.assertEqual(final_state,
actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def publish_tracked_objects(self, now):
"""
Publish markers of tracked objects to Rviz
"""
# Make sure we can get the required transform first:
if self.use_scan_header_stamp_for_tfs:
tf_time = now
try:
self.listener.waitForTransform(self.publish_people_frame, self.fixed_frame, tf_time, rospy.Duration(1.0))
transform_available = True
except:
transform_available = False
else:
tf_time = rospy.Time(0)
transform_available = self.listener.canTransform(self.publish_people_frame, self.fixed_frame, tf_time)
marker_id = 0
if not transform_available:
rospy.loginfo("Person tracker: tf not avaiable. Not publishing people")
else:
for track in self.objects_tracked:
if track.is_person:
continue
if self.publish_occluded or track.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
# Get the track position in the <self.publish_people_frame> frame
ps = PointStamped()
ps.header.frame_id = self.fixed_frame
ps.header.stamp = tf_time
ps.point.x = track.pos_x
ps.point.y = track.pos_y
try:
ps = self.listener.transformPoint(self.publish_people_frame, ps)
except:
continue
# publish rviz markers
marker = Marker()
marker.header.frame_id = self.publish_people_frame
marker.header.stamp = now
marker.ns = "objects_tracked"
if track.in_free_space < self.in_free_space_threshold:
marker.color.r = track.colour[0]
marker.color.g = track.colour[1]
marker.color.b = track.colour[2]
else:
marker.color.r = 0
marker.color.g = 0
marker.color.b = 0
marker.color.a = 1
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.id = marker_id
marker_id += 1
marker.type = Marker.CYLINDER
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.2
marker.pose.position.z = 0.15
self.marker_pub.publish(marker)
# # Publish a marker showing distance travelled:
# if track.dist_travelled > 1:
# marker.color.r = 1.0
# marker.color.g = 1.0
# marker.color.b = 1.0
# marker.color.a = 1.0
# marker.id = marker_id
# marker_id += 1
# marker.type = Marker.TEXT_VIEW_FACING
# marker.text = str(round(track.dist_travelled,1))
# marker.scale.z = 0.1
# marker.pose.position.z = 0.6
# self.marker_pub.publish(marker)
# Publish <self.confidence_percentile>% confidence bounds of track as an ellipse:
# cov = track.filtered_state_covariances[0][0] + track.var_obs # cov_xx == cov_yy == cov
# std = cov**(1./2.)
# gate_dist_euclid = scipy.stats.norm.ppf(1.0 - (1.0-self.confidence_percentile)/2., 0, std)
# marker.type = Marker.SPHERE
# marker.scale.x = 2*gate_dist_euclid
# marker.scale.y = 2*gate_dist_euclid
# marker.scale.z = 0.01
# marker.color.r = 1.0
# marker.color.g = 1.0
# marker.color.b = 1.0
# marker.color.a = 0.1
# marker.pose.position.z = 0.0
# marker.id = marker_id
# marker_id += 1
# self.marker_pub.publish(marker)
# Clear previously published track markers
for m_id in xrange(marker_id, self.prev_track_marker_id):
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = self.publish_people_frame
marker.ns = "objects_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_track_marker_id = marker_id
def capture(self, request, fname=None):
rospy.logout('sim_capture: New capture initiated @ %3.2f.' %
rospy.Time.now().to_sec())
rospy.logout(
'sim_capture: Moving to <%3.2f, %3.2f, %3.2f> %3.2f-deg' %
(request.x, request.y, request.z, math.degrees(request.ang)))
self.navstack(request.x, request.y, request.z, request.ang)
rospy.logout('sim_capture: Arrived at location.')
rospy.logout('sim_capture: Initializing recorder and servoing.')
ps = PointStamped()
ps.header.frame_id = '/base_link'
ps2 = PointStamped()
ps2.header.frame_id = '/base_link'
ps2.point.x = 0.1
# Begin Servoing. Warning: Hack!
sm = ServoMode(self.follow1)
#bp = BasePose( )
pts = []
t0 = rospy.Time.now().to_sec()
while sm.done == False:
if rospy.Time.now().to_sec() - t0 > 180:
rospy.logout('sim_capture: Time up. Aborting.')
self.abort()
try:
ps.header.stamp = rospy.Time(0)
ps_map = self.listener.transformPoint('/map', ps)
x = ps_map.point.x
y = ps_map.point.y
ps2.header.stamp = rospy.Time(0)
ps2_map = self.listener.transformPoint('/map', ps2)
pts.append([x, y, ps2_map.point.x, ps2_map.point.y])
inside = x > -0.5 and x < 10.5 and y > -0.5 and y < 6.5
if not inside:
self.abort()
except:
rospy.logout('sim_capture: Failed transform.')
pass
rospy.sleep(0.2)
rospy.logout('sim_capture: Done servoing.')
#pts = list( bp.pts )
sm.stop()
#bp.stop()
# Stop recorder and shuttle to disk.
if fname:
f = open(fname, 'w')
else:
f = open('trajectory_caps/' + str(int(time.time())) + '_cap.pkl',
'w')
pkl.dump(pts, f)
f.close()
rospy.logout('sim_capture: Capture complete')
return int(True)
def prepare_geometry_msg(self):
'''
Fill point message
'''
point_msg = Point()
point_msg.x = 1.0;
point_msg.y = 1.0;
point_msg.z = 1.0;
'''
Fill point message
'''
point_stamped_msg = PointStamped()
now = rospy.get_rostime()
#rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
point_stamped_msg.header.stamp = now;
point_stamped_msg.header.frame_id = "frame_dummy";
point_stamped_msg.point.x = 1.0;
point_stamped_msg.point.y = 1.0;
point_stamped_msg.point.z = 1.0;
'''
Fill twist message
'''
twist_msg = Twist()
twist_msg.linear.x = 1.0;
twist_msg.linear.y = 0.0;
twist_msg.linear.z = 0.0;
twist_msg.angular.x = 0.0;
twist_msg.angular.y = 0.0;
twist_msg.angular.z = 0.0;
'''
Fill pose 2D message
Please, do it your self for practice
'''
pose_2d_msg = Pose2D()
pose_2d_msg.x = 1.0
pose_2d_msg.y = 2.0
pose_2d_msg.theta = 0.0
rospy.loginfo("Pose2D values: x = %f, y = %f, theta = %f"
% (pose_2d_msg.x, pose_2d_msg.y, pose_2d_msg.theta))
'''
Fill pose stamped message
Please, do it your self for practice
'''
pose_stamped_msg = PoseStamped()
header_sub_msg = Header()
header_sub_msg.seq = 0
header_sub_msg.stamp = rospy.get_rostime()
header_sub_msg.frame_id = "world_frame"
point_sub_msg = Point()
point_sub_msg.x = 2.0
point_sub_msg.y = 2.0
point_sub_msg.z = 2.0
quart_sub_msg = Quaternion()
quart_sub_msg.x = 3.0
quart_sub_msg.y = 4.0
quart_sub_msg.z = 5.0
quart_sub_msg.w = 6.0
pose_sub_msg = Pose()
pose_sub_msg.position = point_sub_msg
pose_sub_msg.orientation = quart_sub_msg
pose_stamped_msg.pose = pose_sub_msg
pose_stamped_msg.header = header_sub_msg
rospy.loginfo("Quaternion w: %f" % pose_stamped_msg.pose.orientation.w)
'''
Fill transform message
Please, do it your self for practice
'''
transform_msg = Transform()
# add transform between camera and cf system!?
#rospy.loginfo("updated position with slam pose")
msg.point.x = transform.pose.position.x
msg.point.y = transform.pose.position.y
msg.point.z = transform.pose.position.z
pub.publish(msg)
if __name__ == '__main__':
rospy.init_node('publish_external_position_vicon', anonymous=True)
topic = rospy.get_param("~position_topic", "")
rospy.wait_for_service('update_params')
rospy.loginfo("found update_params service")
update_params = rospy.ServiceProxy('update_params', UpdateParams)
firstTransform = True
msg = PointStamped()
msg.header.seq = 0
msg.header.stamp = rospy.Time.now()
pub = rospy.Publisher("external_position", PointStamped, queue_size=1)
rospy.Subscriber(topic, PoseStamped, onNewTransform)
rospy.loginfo("subscribed to slam pose")
rospy.spin()
import actionlib
import tf2_ros
import tf2_geometry_msgs
import utm
rospy.init_node('GPS_goals')
tf_buffer = tf2_ros.Buffer(rospy.Duration(10))
tf_listener = tf2_ros.TransformListener(tf_buffer)
client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
client.wait_for_server()
gps_pub = rospy.Publisher('/GPS_odom_point', PointStamped, queue_size=10, latch=True)
utm_point = PointStamped()
utm_point.header.frame_id = "utm"
odom_point = PointStamped()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = 0
goal.target_pose.pose.orientation.w = 1
def UTM_point(msg):
global utm_point, odom_point, goal
print msg
utm_point.point = msg
utm_point.header.stamp = rospy.Time.now()
def main():
global home_yaw, home_yaw_recorded, home_x, home_y, current_x, current_y, current_yaw, limit_x, limit_y, n, t, cached_var, ang_vel_lim, lin_vel_lim
global limit_x, limit_y, obstacles, obs_x, obs_y, obs_r, max_time, min_time, counter, total_time
xAnt = yAnt = 0
rospy.init_node('Turtle_Controller')
rate = rospy.Rate(10.0)
#Lidar odometry
# rospy.Subscriber("/odom_rf2o", Odometry, odom_cb)
#Wheel encoder odometry
rospy.Subscriber("/odom", Odometry, odom_cb)
rospy.Subscriber("/move_base_simple/goal", PoseStamped, calc_target_cb)
rospy.Subscriber("/battery_state", BatteryState, batt_voltage_cb)
rospy.Subscriber("/raw_obstacles", Obstacles, obstacles_cb)
rospy.Subscriber("/gazebo/model_states", ModelStates, model_state_cb)
pub = rospy.Publisher('destination_point', PointStamped, queue_size=1)
pub2 = rospy.Publisher('cmd_vel', Twist, queue_size=5)
pub3 = rospy.Publisher('boundary_cube', Marker, queue_size=1)
pub4 = rospy.Publisher('mpc_path', Path, queue_size=1)
pub5 = rospy.Publisher('turtle_point', PointStamped, queue_size=1)
mpc_path = Path()
while not rospy.is_shutdown():
if home_yaw_recorded is False and current_yaw != 0:
home_yaw = current_yaw
home_yaw_recorded = True
ready = select.select([sys.stdin], [], [], 0)[0]
if ready:
x = ready[0].read(1)
if x == 'x':
sys.stdin.flush()
limits_x = float(raw_input('Enter x limit:'))
if x == 'y':
sys.stdin.flush()
limit_y = float(raw_input('Enter y limit:'))
if x == 'n':
sys.stdin.flush()
n = int(raw_input("Enter nsteps:"))
if x == 't':
sys.stdin.flush()
t = float(raw_input("Enter timestep duration:"))
# current_r = math.sqrt(current_x * current_x + current_y * current_y)
destination_r = math.sqrt(
math.pow(destination_x - current_x, 2) +
math.pow(destination_y - current_y, 2))
# limit_r = math.sqrt(limit_x * limit_x + limit_y * limit_y)
# this is for controlling the turtle bot, mpc solver only yields paths in cartesian space.
dx = destination_x - current_x
dy = destination_y - current_y
current_pose = [dx, dy, 0]
timer = time.time()
#Calls to the MPC solver
try:
velocity_x_des, velocity_y_des, cached_var = MPC_solver(
init_pose,
current_pose,
final_pose,
nsteps=n,
interval=t,
variables=cached_var,
vehicle_r=vehicle_r,
obstacles=obstacles)
except ValueError:
velocity_x_des = 0
velocity_y_des = 0
current_time = time.time() - timer
if (current_time > max_time):
max_time = current_time
if (current_time < min_time):
min_time = current_time
total_time += current_time
counter = counter + 1
avg_time = total_time / counter
if (counter > 100000):
total_time = 0.
counter = 0
print "Average time = %f \t Max time = %f \t Min time = %f" % (
avg_time, max_time, min_time)
# print(time.time() - timer)
x_arr = cached_var.get("solution")[1:n + 1]
y_arr = cached_var.get("solution")[2 * n + 2:2 * n + 1 + n + 1]
velocity_x_des = np.clip(velocity_x_des, -lin_vel_lim, lin_vel_lim)
velocity_y_des = np.clip(velocity_y_des, -lin_vel_lim, lin_vel_lim)
x_e = x_arr[1] - x_arr[0]
y_e = y_arr[1] - y_arr[0]
destination_yaw = math.atan2(y_e, x_e) * 180 / 3.1416
current_yaw = 360.0 + current_yaw if current_yaw < 0 else current_yaw
destination_yaw = 360.0 + destination_yaw if destination_yaw < 0 else destination_yaw
#This part controls the yaw of the turtlebot, taking into account the shortest direction for the desired yaw
if (math.fabs(destination_yaw - current_yaw) > 180):
if (destination_yaw > current_yaw):
temp_desired_yaw = destination_yaw - current_yaw
temp_current_yaw = 361
elif current_yaw > destination_yaw:
temp_current_yaw = current_yaw - destination_yaw
temp_desired_yaw = 361
velocity_yaw_des = np.clip(temp_desired_yaw - temp_current_yaw,
-ang_vel_lim, ang_vel_lim)
else:
velocity_yaw_des = np.clip(destination_yaw - current_yaw,
-ang_vel_lim, ang_vel_lim)
move_cmd = Twist()
if (destination_r < 0.1):
move_cmd.linear.x = 0
move_cmd.angular.z = 0
elif (math.fabs(destination_yaw - ((current_yaw - 180) % 360)) > 20):
move_cmd.linear.x = 0
move_cmd.angular.z = is_simulation * velocity_yaw_des * 2
else:
move_cmd.linear.x = math.sqrt(velocity_x_des**2 +
velocity_y_des**2)
move_cmd.angular.z = is_simulation * velocity_yaw_des
pub2.publish(move_cmd)
#From hereon, deals with visualization in RViz
destination_point = PointStamped(header=Header(
stamp=rospy.get_rostime()))
destination_point.header.frame_id = 'local_origin'
destination_point.point.x = destination_x - home_x
destination_point.point.y = destination_y - home_y
destination_point.point.z = 0
pub.publish(destination_point)
turtle_point = PointStamped(header=Header(stamp=rospy.get_rostime()))
turtle_point.header.frame_id = 'local_origin'
turtle_point.point.x = current_x
turtle_point.point.y = current_y
turtle_point.point.z = 0
pub5.publish(turtle_point)
boundary_cube = Marker()
boundary_cube.header.frame_id = 'local_origin'
boundary_cube.header.stamp = rospy.Time.now()
boundary_cube.action = boundary_cube.ADD
boundary_cube.type = boundary_cube.CUBE
boundary_cube.id = 0
boundary_cube.scale.x = limit_x * 2
boundary_cube.scale.y = limit_y * 2
boundary_cube.scale.z = 1
boundary_cube.color.a = 0.5
boundary_cube.color.r = 0.4
boundary_cube.color.g = 0.2
boundary_cube.color.b = 0
pub3.publish(boundary_cube)
if True:
mpc_pose_array = [None] * n
for i in range(0, n):
mpc_pose = PoseStamped()
mpc_pose.header.seq = i
mpc_pose.header.stamp = rospy.Time.now() + rospy.Duration(
t * i)
mpc_pose.header.frame_id = "local_origin"
mpc_pose.pose.position.x = -x_arr[i] + destination_x
mpc_pose.pose.position.y = -y_arr[i] + destination_y
mpc_pose_array[i] = mpc_pose
if (xAnt != mpc_pose.pose.position.x
and yAnt != mpc_pose.pose.position.y):
mpc_path.header.frame_id = "local_origin"
mpc_path.header.stamp = rospy.Time.now()
mpc_path.poses = mpc_pose_array
xAnt = mpc_pose.pose.orientation.x
yAnt = mpc_pose.pose.position.y
pub4.publish(mpc_path)
br.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), "odom",
"local_origin")
rospy.sleep(0.3)
rospy.loginfo("Getting a TransformListener...")
tf_listener = tf.TransformListener()
# Waiting a moment so the tf_listener catches some transform
rospy.sleep(0.5)
# Check if frames exist
if not tf_listener.frameExists(frame_1):
print frame_1 + " does not exist on current TF."
exit(0)
if not tf_listener.frameExists(frame_2):
print frame_2 + " does not exist on current TF."
exit(0)
# Get the point of the tip of the left hand index
ps = PointStamped()
ps.point = Point(0.0, 0.0, 0.0)
ps.header.frame_id = frame_1
ps.header.stamp = rospy.Time(0) # For getting last transform
# Transform this point to the frame reference of
# right hand index
got_transform = False
while not got_transform:
try:
tps = tf_listener.transformPoint(frame_2, ps)
got_transform = True
except:
print "Transformation failed, waiting 0.3 and retrying"
rospy.sleep(0.3)
return math.atan2(local_frame_goal.y, local_frame_goal.x)
def convert_to_local_frame(self, stamped_point):
self.tf_listener.waitForTransform("/base_link",
stamped_point.header.frame_id,
rospy.Time(), rospy.Duration(4))
return self.tf_listener.transformPoint("/base_link", stamped_point)
def go_to(self, destination):
local_frame_goal = self.convert_to_local_frame(destination).point
while self.euclidean_distance(local_frame_goal) > 0.05:
local_frame_goal = self.convert_to_local_frame(destination).point
vel_msg = Twist()
vel_msg.linear.x = self.euclidean_distance(local_frame_goal)
vel_msg.angular.z = self.angular_velocity(local_frame_goal)
self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
if __name__ == "__main__":
try:
robot = AToB()
destination = PointStamped(header=Header(stamp=rospy.Time.now(),
frame_id="/odom"),
point=Point(-12.9481, 22.9615, 0.0))
robot.go_to(destination)
except rospy.ROSInterruptException:
pass
