def test_updateState_PosOffsetPosArc(self):
'''
Test the robot following a path starting from the same angle
but with a positive offset and larger radius
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.ARC
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi / 2.0
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
init_quat = quaternion_from_euler(0, 0, math.pi + math.pi / 2)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 1.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
maxIter = 1000
count = 1
rhoDes = pathSeg.curvature - .3
angle = State.getYaw(pathSeg.init_tan_angle)
r = 1 / abs(rhoDes)
startAngle = angle - math.pi / 2.0
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
dAng = pathSeg.seg_length * (count / (maxIter / 2.0)) * rhoDes
arcAng = startAngle + dAng
point.x = pathSeg.ref_point.x + r * math.cos(arcAng)
point.y = pathSeg.ref_point.y + r * math.sin(arcAng)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def filter_invalid_scan_poses(self, cell_x, cell_y, pose_list):
"""
Filters poses, that are behind obstacles or unknown which nearby the given cell.
:param cell_x: x coordinate of the cell
:type: float
:param cell_y: y coordinate of the cell
:type: float
:param pose_list: list of poses
:type: [PoseStamped]
:return: filtered list of poses
:type: [PoseStamped]
"""
poses = deepcopy(pose_list)
surr_cells = self.get_surrounding_cells8(cell_x, cell_y)
for (c, x, y) in surr_cells:
if not c.is_free():
p = Point(x, y, 0)
p2 = Point(cell_x, cell_y, 0)
cell_to_p = subtract_point(p, p2)
for pose in pose_list:
x_under_pose = pose.pose.position.x
y_under_pose = pose.pose.position.y
# filter poses that are pointing to unknowns or obstacles
cell_to_pose = subtract_point(
Point(x_under_pose, y_under_pose, 0), p2)
angle = get_angle(cell_to_p, cell_to_pose)
if angle <= pi / 4 + 0.0001 and pose in poses:
poses.remove(pose)
return poses
def test_updateState_NegOffsetNegArc(self):
'''
Test the robot following a path starting from the same angle
but with a negative offset and a smaller radius
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.ARC
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi/2.0
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
init_quat = quaternion_from_euler(0,0,3*math.pi/4.0-math.pi/2)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = -1.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
maxIter = 1000
count = 1
rhoDes = pathSeg.curvature + .3
angle = State.getYaw(pathSeg.init_tan_angle)
r=1/abs(rhoDes)
startAngle = angle + math.pi/2.0
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
dAng = pathSeg.seg_length*(count/(maxIter/2.0))*rhoDes
arcAng = startAngle+dAng
point.x = pathSeg.ref_point.x + r*math.cos(arcAng)
point.y = pathSeg.ref_point.y + r*math.sin(arcAng)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def to_marker(self):
"""
:return: a marker representing the map.
:type: Marker
"""
marker = Marker()
marker.type = Marker.CUBE_LIST
for x in range(0, len(self.field)):
for y in range(0, len(self.field[0])):
marker.header.frame_id = '/odom_combined'
marker.ns = 'suturo_planning/map'
marker.id = 23
marker.action = Marker.ADD
(x_i, y_i) = self.index_to_coordinates(x, y)
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.w = 1
marker.scale.x = self.cell_size
marker.scale.y = self.cell_size
marker.scale.z = 0.01
p = Point()
p.x = x_i
p.y = y_i
marker.points.append(p)
c = self.get_cell_by_index(x, y)
marker.colors.append(self.__cell_to_color(c))
marker.lifetime = rospy.Time(0)
return marker
def test_stop(self):
'''
Test the stop method
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2)*2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0,0,math.pi/4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 1.5
point = PointMsg()
maxIter = 300
count = 1
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = pathSeg.seg_length*(count/(maxIter/2.0))*math.cos(math.pi/4.0)
point.y = pathSeg.seg_length*(count/(maxIter/2.0))*math.sin(math.pi/4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
self.assertEquals(state.v, 0.5)
self.assertEquals(state.o, 1.5)
state.stop()
self.assertEquals(state.v, 0.0)
self.assertEquals(state.o, 0.0)
def to_marker(self):
"""
:return: a marker representing the map.
:type: Marker
"""
marker = Marker()
marker.type = Marker.CUBE_LIST
for x in range(0, len(self.field)):
for y in range(0, len(self.field[0])):
marker.header.frame_id = '/odom_combined'
marker.ns = 'suturo_planning/map'
marker.id = 23
marker.action = Marker.ADD
(x_i, y_i) = self.index_to_coordinates(x, y)
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.w = 1
marker.scale.x = self.cell_size
marker.scale.y = self.cell_size
marker.scale.z = 0.01
p = Point()
p.x = x_i
p.y = y_i
marker.points.append(p)
c = self.get_cell_by_index(x, y)
marker.colors.append(self.__cell_to_color(c))
marker.lifetime = rospy.Time(0)
return marker
def main(x=None,y=None):
global goal
rospy.init_node('astar_alpha_goal_publisher')
goalPub = rospy.Publisher('goal_point',GoalMsg)
naptime = rospy.Rate(RATE)
goal = GoalMsg()
goal.new = True
if(x is None or y is None):
goal.none = True
else:
goal_point = PointMsg()
goal_point.x = x
goal_point.y = y
goal.goal = goal_point
while not rospy.is_shutdown():
goalPub.publish(goal)
goal.new = False
naptime.sleep()
def test_updateState_NegOffsetNegSpin(self):
'''
Test the robot following a path starting from an angle with
a positive offset
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.SPIN_IN_PLACE
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0, 0, 0.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 1.0
ref_point = PointMsg()
ref_point.x = 1.0
ref_point.y = 1.0
pathSeg.ref_point = ref_point
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.0
vel_cmd.angular.z = 0.5
point = PointMsg()
psi = 0.0
maxIter = 1000
count = 1
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
psi = 2.0 * state.pathSeg.seg_length * count / maxIter + math.pi / 4.0
state.updateState(vel_cmd, point, psi)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetNegSpin(self):
'''
Test the robot following a path starting from an angle with
a positive offset
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.SPIN_IN_PLACE
pathSeg.seg_number = 1
pathSeg.seg_length = math.pi
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0,0,0.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 1.0
ref_point = PointMsg()
ref_point.x = 1.0
ref_point.y = 1.0
pathSeg.ref_point = ref_point
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.0
vel_cmd.angular.z = 0.5
point = PointMsg()
psi = 0.0
maxIter = 1000
count = 1
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
psi = 2.0*state.pathSeg.seg_length*count/maxIter + math.pi/4.0
state.updateState(vel_cmd, point, psi)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetLine(self):
'''
Test the robot following a path starting with a negative
offset and crossing over the path
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2) * 2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0, 0, math.pi / 4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
actSegLength = 2.0 * math.sqrt(2) + 1.0
maxIter = 1000
count = 1
# extrapolate next point
while (state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = actSegLength * (count / (maxIter / 2.0)) * math.cos(
3 * math.pi / 4.0) + 1.0
point.y = actSegLength * (count / (maxIter / 2.0)) * math.sin(
3 * math.pi / 4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def test1_1(self):
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1,0,0)
p.pose.orientation = euler_to_quaternion(0,pi,0)
p2 = Point(0.7815,0,0)
p1 = get_fingertip(p)
self.assertTrue(abs(p1.point.x - p2.x) < 0.0001)
self.assertTrue(abs(p1.point.y - p2.y) < 0.0001)
self.assertTrue(abs(p1.point.z - p2.z) < 0.0001)
def getPoint(self): t = self.getTranslation() p = Point() p.x = t[0] p.y = t[1] p.z = t[2] return p #eof #eoc
def main():
global corner1, corner2, numCells
global brush
global position
global pointList
rospy.init_node('brushfire_alpha_main')
corner1 = (-6.25, 8.2)
corner2 = (15.75, 28.2)
numCells = 100
brush = BrushFire(corner1, corner2, numCells, size=15)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCels: "
print numCells
print ""
rospy.Subscriber('goal_point', GoalMsg, goalCallback)
rospy.Subscriber('/costmap_alpha/costmap/obstacles', GridCellsMsg,
obstaclesCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list', PointListMsg)
pointList = PointListMsg()
t = Timer(2.0, resetPath)
t.start()
while not rospy.is_shutdown():
if (position is None or brush is None or brush.goal is None):
naptime.sleep()
continue
brush.extractLocal(position.x, position.y)
brush.brushfire()
brush.computePath()
pointList.points = []
for point in brush.pathList:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
pointList.new = False
naptime.sleep()
def main():
global corner1, corner2, numCells
global brush
global position
global pointList
rospy.init_node('brushfire_alpha_main')
corner1 = (-6.25,8.2)
corner2 = (15.75,28.2)
numCells = 100
brush = BrushFire(corner1,corner2,numCells,size=15)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCels: "
print numCells
print ""
rospy.Subscriber('goal_point',GoalMsg,goalCallback)
rospy.Subscriber('/costmap_alpha/costmap/obstacles', GridCellsMsg,obstaclesCallback)
rospy.Subscriber('map_pos',PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list',PointListMsg)
pointList = PointListMsg()
t = Timer(2.0, resetPath)
t.start()
while not rospy.is_shutdown():
if(position is None or brush is None or brush.goal is None):
naptime.sleep()
continue
brush.extractLocal(position.x,position.y)
brush.brushfire()
brush.computePath()
pointList.points = []
for point in brush.pathList:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
pointList.new = False
naptime.sleep()
def test_updateState_NegOffsetLine(self):
'''
Test the robot following a path starting with a negative
offset and crossing over the path
'''
pathSeg = PathSegmentMsg()
pathSeg.seg_type = pathSeg.LINE
pathSeg.seg_number = 1
pathSeg.seg_length = math.sqrt(2)*2
pathSeg.ref_point.x = 0.0
pathSeg.ref_point.y = 0.0
pathSeg.ref_point.z = 0.0
init_quat = quaternion_from_euler(0,0,math.pi/4.0)
pathSeg.init_tan_angle.w = init_quat[3]
pathSeg.init_tan_angle.x = init_quat[0]
pathSeg.init_tan_angle.y = init_quat[1]
pathSeg.init_tan_angle.z = init_quat[2]
pathSeg.curvature = 0.0
maxSpeed = TwistMsg()
maxSpeed.linear.x = 1.0
maxSpeed.angular.z = 1.0
pathSeg.max_speeds = maxSpeed
minSpeed = TwistMsg()
pathSeg.min_speeds = minSpeed
pathSeg.accel_limit = 1.0
pathSeg.decel_limit = -1.0
state = State(pathSeg)
vel_cmd = TwistMsg()
vel_cmd.linear.x = 0.5
vel_cmd.angular.z = 0.0
point = PointMsg()
actSegLength = 2.0*math.sqrt(2)+1.0
maxIter = 1000
count = 1
# extrapolate next point
while(state.segDistDone < 1.0 and maxIter > count):
# create where the robot should have moved
point.x = actSegLength*(count/(maxIter/2.0))*math.cos(3*math.pi/4.0) + 1.0
point.y = actSegLength*(count/(maxIter/2.0))*math.sin(3*math.pi/4.0)
state.updateState(vel_cmd, point, 0.0)
count += 1
self.assertTrue(count < maxIter)
self.assertTrue(state.segDistDone >= 1.0)
def getPoint(self):
t = self.getTranslation()
p = Point()
p.x = t[0]
p.y = t[1]
p.z = t[2]
return p
#eof
#eoc
def remove_puzzle_fixture(self, yaml):
rospy.logdebug("remove_puzzle_fixture called!")
if yaml.task_type == Task.TASK_5:
#fixture_position = deepcopy(yaml.puzzle_fixture.position)
#fixture_position = add_point(fixture_position, Point(0.115 if yaml.puzzle_fixture.position.x < 0 else -0.18, 0.165 if yaml.puzzle_fixture.position.y < 0 else 0.05, 0))
fixture_position = get_puzzle_fixture_center(yaml.puzzle_fixture)
for x in range(0, len(self.field)):
for y in range(0, len(self.field[x])):
cell = self.get_cell_by_index(x, y)
cell_coor_2d = self.index_to_coordinates(x, y)
cell_x, cell_y = cell_coor_2d
cell_coor = Point(cell_x, cell_y, 0)
fixture_dist = euclidean_distance_in_2d(
fixture_position, cell_coor)
if fixture_dist < 0.35:
rospy.logdebug("cell at (" + str(x) + "," + str(y) +
") marked as fixture cell")
cell.set_mark()
if cell.is_unknown():
cell.highest_z = 0.1
cell.average_z = 0.1
cell.set_obstacle()
if cell.highest_z > 0.1:
cell.highest_z = 0.1
if cell.average_z > 0.1:
cell.average_z = 0.1
rospy.logdebug("the cell at (" + str(x) + "," +
str(y) + "): " + str(cell))
self.publish_as_marker()
def get_message(self):
object_id = self.object_name.split("_")[-1]
msg = ModelDescription()
# generate InstanceId
msg.instance_id = InstanceId()
msg.instance_id.class_name = self.object_type
msg.instance_id.id = object_id
# HACK this information should be in some ontology
if 'ProductWithAN' in self.object_type:
msg.instance_id.ns = '/IAISupermarket/Catalog'
elif 'ShelfLabel' in self.object_type:
msg.instance_id.ns = '/IAISupermarket/ShelfLabels'
else:
msg.instance_id.ns = '/IAISupermarket/Shelves'
# generate MeshDescription
# NOTE: default is to use the class name
msg.mesh_description = MeshDescription()
msg.mesh_description.path_to_mesh = ''
msg.mesh_description.path_to_material = ''
# generate Tag's
msg.tags.append(self.get_tag('SemLog', 'LogType', 'Static'))
msg.tags.append(self.get_tag('SemLog', 'Id', object_id))
msg.tags.append(self.get_tag('SemLog', 'Class', self.object_type))
# set the pose
msg.pose = Pose()
msg.pose.position = Point()
msg.pose.position.x = self.transform[2][0]
msg.pose.position.y = self.transform[2][1]
msg.pose.position.z = self.transform[2][2]
msg.pose.orientation = Quaternion()
msg.pose.orientation.x = self.transform[3][0]
msg.pose.orientation.y = self.transform[3][1]
msg.pose.orientation.z = self.transform[3][2]
msg.pose.orientation.w = self.transform[3][3]
return msg
def get_yaw(q):
"""
Calculates the pitch of a quaternion.
:param q: Quaternion
:type: Quaternion
:return: yaw of the quaternion
:type: float
"""
gripper_direction = qv_mult(q, Point(1, 0, 0))
v = deepcopy(gripper_direction)
v.z = 0
if 0 <= magnitude(v) <= 0.001:
v = Point(1, 0, 0)
yaw = get_angle(v, Point(1, 0, 0))
return yaw
def goto_shelf2(self):
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-1.0, self.dist_to_shelfs, 0.0)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
self(shelf)
def relative_pose(self, position, orientation):
shelf = PoseStamped()
header = Header()
header.frame_id = 'base_footprint'
shelf.header = header
shelf.pose.position = Point(*position)
shelf.pose.orientation = Quaternion(*orientation)
self(shelf)
def goto_shelf4(self):
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-2.9, self.dist_to_shelfs, 0.000)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
# shelf.pose.orientation = Quaternion(*quaternion_from_euler(0,0,pi*.99))
self(shelf)
def goto_shelf1(self):
# self.client.cancel_all_goals()
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-0.0, self.dist_to_shelfs, 0.0)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
self(shelf)
def np_array_to_pose_stampeds(self, nparray, orientation, header):
pose_list = []
for p in nparray:
pose = PoseStamped()
pose.pose.position = Point(*p)
pose.pose.orientation = orientation
pose.header = header
pose_list.append(pose)
return pose_list
def main(fullList):
global pose
global pathList
rospy.init_node('pathplanner_alpha_dummyNode')
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
dummyPointPub = rospy.Publisher('point_list', PointListMsg)
pathList.new = True
naptime = rospy.Rate(RATE)
#point = PointMsg()
#point.x = pose.pose.position.x
#point.y = pose.pose.position.y
#appendToList(point)
with open(fullList, 'rb') as csvFile:
dialect = csv.Sniffer().sniff(csvFile.read(1024)) # auto detect delimiters
csvFile.seek(0)
reader = csv.reader(csvFile, dialect) # open up a csv reader object with the csv file
for i,row in enumerate(reader):
point1 = PointMsg()
try:
point1.x = float(row[0])
except ValueError:
print "x ValueError"
try:
point1.y = float(row[1])
except ValueError:
print "y ValueError"
appendToList(point1)
while not rospy.is_shutdown():
dummyPointPub.publish(pathList)
pathList.new = False
naptime.sleep()
def make_scan_pose(point, distance, angle, frame="/odom_combined", n=8):
"""
Calculates "n" positions around and pointing to "point" with "distance" in an "angle"
:param point: Point the positions will be pointing to
:type: Point
:param distance: distance from the point to tcp origin
:type: float
:param angle: pitch of the gripper, 0 = horizontally, pi/2 = downwards
:type: float
:param frame: the frame that the positions will have
:type: str
:param n: number of positions
:type: float
:return: list of scan poses
:type: [PoseStamped
"""
#TODO: assumes odom_combined
look_positions = []
alpha = pi/2 - angle
r = sin(alpha) * distance
h = cos(alpha) * distance
h_vector = Point()
h_vector.z = h
muh = add_point(point, h_vector)
for i in range(0, n):
a = 2 * pi * ((i + 0.0) / (n + 0.0))
b = a - (pi / 2)
look_point = Point(cos(a), sin(a), 0)
look_point = set_vector_length(r, look_point)
look_point = add_point(look_point, muh)
roll_point = Point(cos(b), sin(b), 0)
roll_point = add_point(roll_point, point)
look_pose = PoseStamped()
look_pose.header.frame_id = frame
look_pose.pose.orientation = three_points_to_quaternion(look_point, point, roll_point)
look_pose.pose.position = look_point
look_positions.append(look_pose)
look_positions.sort(key=lambda x: magnitude(x.pose.position))
return look_positions
def orientation_to_vector(orientation):
"""
Transforms a 1, 0, 0 vector by a quaternion, the "roll" information is lost.
:param orientation: orientation
:type: Quaternion
:return: vector
:type: Point
"""
return qv_mult(orientation, Point(1, 0, 0))
def multiply_point(s, p):
"""
p * s
:param s: factor
:type: float
:param p: point
:type: Point / (float(x), float(y), float(z))
:return: multiplied point
:type: Point
"""
v = p
if type(p) is Point:
v = (p.x, p.y, p.z)
return Point(*np.multiply(v, s))
def get_fingertip(hand_pose):
"""
Calculates the point between the finger tips, where objects will be hold.
:param hand_pose: hand pose
:type: PoseStamped
:return: point between fingers
:type: PointStamped
"""
grasp_point = PointStamped()
grasp_point.point = set_vector_length(hand_length + finger_length,
Point(1, 0, 0))
grasp_point.point = qv_mult(hand_pose.pose.orientation, grasp_point.point)
grasp_point.point = add_point(hand_pose.pose.position, grasp_point.point)
grasp_point.header.frame_id = hand_pose.header.frame_id
return grasp_point
def test2_1(self):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "muh"
co.header.frame_id = "/odom_combined"
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions.append(0.3)
cylinder.dimensions.append(0.03)
co.primitives = [cylinder]
co.primitive_poses = [Pose()]
co.primitive_poses[0].position = Point(1.2185, 0,0)
co.primitive_poses[0].orientation = Quaternion(0,0,0,1)
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions.append(0.1)
box.dimensions.append(0.1)
box.dimensions.append(0.1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[1].position = Point(1.1185, 0,0)
co.primitive_poses[1].orientation = Quaternion(0,0,0,1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[2].position = Point(0, 0,0)
co.primitive_poses[2].orientation = Quaternion(0,0,0,1)
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1,0,0)
p.pose.orientation = euler_to_quaternion(0,0,0)
self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
def prolog_to_transform(frame_id, child_frame_id, translation, rotation):
"""
:type frame_id: str
:type child_frame_id: str
:type translation: list
:type rotation: list
:rtype: TransformStamped
"""
if child_frame_id==None or translation==None or rotation==None: return None
t = TransformStamped()
t.header.frame_id = frame_id.encode('utf-8')
t.child_frame_id = child_frame_id.encode('utf-8')
t.transform.translation = Point(*translation)
t.transform.rotation = Quaternion(*rotation)
return t
def normalize(p):
"""
Normalizes a point.
:param p: point
:type: Point / (float(x), float(y), float(z))
:return: normalized point
:type: Point
"""
v = p
if type(p) is Point:
v = (p.x, p.y, p.z)
l = magnitude(v)
result = Point(*(x * 1 / l for x in v))
return result
def get_marker(self):
marker = Marker()
marker.header.frame_id = self.ref_frame
marker.type = marker.MESH_RESOURCE
marker.action = Marker.ADD
marker.id = 1337
marker.ns = self.get_short_name()
marker.color = self.color
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.frame_locked = True
marker.pose.position = Point(*self.transform[-2])
marker.pose.orientation = Quaternion(*self.transform[-1])
marker.mesh_resource = self.mesh_path[:-4] + '.dae'
return marker
def get_puzzle_fixture_center(puzzle_fixture):
'''
Calculates the center of the puzzle fixture. Assumed fixture dimensions: 32cm x 32cm x 10cm
:param puzzle_fixture: geometry_msgs/Pose
:return: Point
'''
l = sqrt(2 * 0.30 * 0.30) / 2
v1 = orientation_to_vector(
rotate_quaternion(deepcopy(puzzle_fixture.orientation), 0, 0, pi / 4))
p1 = puzzle_fixture.position
v1norm = normalize(v1)
print("v1 = " + str(v1))
print("v1norm = " + str(v1norm))
p2 = Point(v1norm.x * l, v1norm.y * l, v1norm.z * l)
print("p2 len = " + str(sqrt(p2.x * p2.x + p2.y * p2.y)))
return add_point(p1, p2)
def subtract_point(p1, p2):
"""
p1 - p2
:param p1: first point
:type: Point/ (float(x), float(y), float(z))
:param p2: second point
:type: Point/ (float(x), float(y), float(z))
:return: Point
:type: Point
"""
v1 = p1
if type(p1) is Point:
v1 = (p1.x, p1.y, p1.z)
v2 = p2
if type(p2) is Point:
v2 = (p2.x, p2.y, p2.z)
return Point(*np.subtract(v1, v2))
def cross_product(p1, p2):
"""
p1 x p2
:param p1: first point
:type: Point / (float(x), float(y), float(z))
:param p2: second point
:type: Point / (float(x), float(y), float(z))
:return: cross product
:type: Point
"""
v1 = p1
if type(p1) is Point:
v1 = (p1.x, p1.y, p1.z)
v2 = p2
if type(p2) is Point:
v2 = (p2.x, p2.y, p2.z)
return Point(*np.cross(v1, v2))
def calculate_grasp_position_box(collision_object, n=8):
'''
Calculates grasp positions for a Box.
:param collision_object: box
:type: CollisionObject
:param n: number of grasps around each side
:type: int
:return: list of possible graps
:type: [PoseStamped]
'''
grasp_positions = []
depth = finger_length
x_len = collision_object.primitives[0].dimensions[shape_msgs.msg.SolidPrimitive.BOX_X] / 2.0
if finger_length < x_len:
depth = x_len
depth_x = depth + hand_length
y_len = collision_object.primitives[0].dimensions[shape_msgs.msg.SolidPrimitive.BOX_Y] / 2.0
if finger_length < y_len:
depth = y_len
depth_y = depth + hand_length
z_len = collision_object.primitives[0].dimensions[shape_msgs.msg.SolidPrimitive.BOX_Z] / 2.0
if finger_length < z_len:
depth = z_len
depth_z = depth + hand_length
#TODO: abfangen wenn eine Seite zu gross ist
#TODO: cooler machen
for i in range(0, n):
a = 2 * pi * ((i + 0.0) / (n + 0.0))
grasp_point = Point(cos(a), sin(a), 0)
d = (abs(grasp_point.x) * depth_x + abs(grasp_point.y) * depth_y + abs(grasp_point.z) * depth_z) / (abs(
grasp_point.x) + abs(grasp_point.y) + abs(grasp_point.z))
grasp_positions.append(make_grasp_pose(d, grasp_point, Point(0,0,1),
collision_object.id))
grasp_point = Point(cos(a), 0, sin(a))
d = (abs(grasp_point.x) * depth_x + abs(grasp_point.y) * depth_y + abs(grasp_point.z) * depth_z) / (abs(
grasp_point.x) + abs(grasp_point.y) + abs(grasp_point.z))
grasp_positions.append(make_grasp_pose(d, grasp_point, Point(0,1,0),
collision_object.id))
grasp_point = Point(0, cos(a), sin(a))
d = (abs(grasp_point.x) * depth_x + abs(grasp_point.y) * depth_y + abs(grasp_point.z) * depth_z) / (abs(
grasp_point.x) + abs(grasp_point.y) + abs(grasp_point.z))
grasp_positions.append(make_grasp_pose(d, grasp_point, Point(1,0,0),
collision_object.id))
return grasp_positions
def test11_1(self):
m = self.make_free_map()
m.get_cell_by_index(9, 9).set_unknown()
m.get_cell_by_index(9, 10).set_unknown()
m.get_cell_by_index(9, 11).set_unknown()
m.get_cell_by_index(10, 9).set_unknown()
m.get_cell_by_index(10, 10).set_unknown()
m.get_cell_by_index(10, 11).set_unknown()
m.get_cell_by_index(11, 9).set_unknown()
m.get_cell_by_index(11, 10).set_unknown()
m.get_cell_by_index(11, 11).set_unknown()
(x, y) = m.index_to_coordinates(10, 10)
poses = make_scan_pose(Point(x, y, 0), 0.05, 0, n=16)
poses = m.filter_invalid_scan_poses(x, y, poses)
# print len(poses)
self.assertTrue(len(poses) == 0, poses)
def getStartAndEndPoints():
if(currSeg.seg_type == PathSegmentMsg.LINE):
p_s = PointMsg()
p_f = PointMsg()
p_s.x = currSeg.ref_point.x
p_s.y = currSeg.ref_point.y
p_f.x = currSeg.ref_point.x + currSeg.seg_length*cos(getYaw(currSeg.init_tan_angle))
p_f.y = currSeg.ref_point.y + currSeg.seg_length*sin(getYaw(currSeg.init_tan_angle))
return (p_s,p_f)
elif(currSeg.seg_type == PathSegmentMsg.ARC):
return (0.0,0.0) # TODO
elif(currSeg.seg_type == PathSegmentMsg.SPIN_IN_PLACE):
return (currSeg.ref_point,currSeg.ref_point) # spins should never move in x,y plane
else:
return (0.0,0.0) # not sure if this is the best default answer
def main():
global corner1, corner2, numCells
global searcher, newPath
rospy.init_node('astar_alpha_main')
# set the parameters specified within the launch files
# corner 1 and corner 2 specify the area in the map
# that astar will consider
# Corner1
if rospy.has_param('corner1x'):
corner1x = rospy.get_param('corner1x')
else:
corner1x = -6.25
if rospy.has_param('corner1y'):
corner1y = rospy.get_param('corner1y')
else:
corner1y = 8.2
# Corner2
if rospy.has_param('corner2x'):
corner2x = rospy.get_param('corner2x')
else:
corner2x = 15.75
if rospy.has_param('corner2y'):
corner2y = rospy.get_param('corner2y')
else:
corner2y = 28.2
corner1 = (corner1x,corner1y)
corner2 = (corner2x,corner2y)
# numCells specifies the number of cells to divide the
# specified astar search space into
if rospy.has_param('numCells'):
numCells = rospy.get_param('numCells')
else:
numCells = 100
# topic that the node looks for the closed points on
if rospy.has_param('inflatedTopic'):
inflatedTopic = rospy.get_param('inflatedTopic')
else:
inflatedTopic = '/costmap_local_alpha/costmap_local/inflated_obstacles'
# topic that the node looks for goal messages on
if rospy.has_param('goalTopic'):
goalTopic = rospy.get_param('goalTopic')
else:
goalTopic = 'goal_point'
# initialize an instance of the Astar class
searcher = Astar(corner1,corner2,numCells)
naptime = rospy.Rate(RATE)
print "corner1: "
print corner1
print ""
print "corner2: "
print corner2
print ""
print "numCells: %i" % numCells
print ""
print "goal topics: %s" % goalTopic
print ""
print "inflatedTopic: %s" % inflatedTopic
rospy.Subscriber(goalTopic,GoalMsg,goalCallback)
rospy.Subscriber(inflatedTopic,GridCellsMsg,inflatedObstaclesCallback)
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
pathPointPub = rospy.Publisher('point_list', PointListMsg)
first_run = True
pointList = PointListMsg()
while not rospy.is_shutdown():
pointList.new = newPath
print "searcher.start"
print searcher.start
print ""
print "searcher.goal"
print searcher.goal
print ""
print "newPath"
print newPath
print ""
print "searcher.path"
print searcher.path
print ""
pointList.points = []
for point in searcher.path:
pathPoint = PointMsg()
pathPoint.x = point[0]
pathPoint.y = point[1]
pointList.points.append(pathPoint)
pathPointPub.publish(pointList)
if newPath:
newPath = False
naptime.sleep()
def main():
global RATE
global lastVCmd
global lastOCmd
global obs
global obsDist
global obsExists
global stopped
global seg_number
global currSeg
global nextSeg
global pose
global ping_angle
rospy.init_node('velocity_profiler_alpha')
desVelPub = rospy.Publisher('des_vel',TwistMsg) # Steering reads this and adds steering corrections on top of the desired velocities
segStatusPub = rospy.Publisher('seg_status', SegStatusMsg) # Lets the other nodes know what path segment the robot is currently executing
rospy.Subscriber("motors_enabled", BoolMsg, eStopCallback) # Lets velocity profiler know the E-stop is enabled
rospy.Subscriber("obstacles", ObstaclesMsg, obstaclesCallback) # Lets velocity profiler know where along the path there is an obstacle
rospy.Subscriber("cmd_vel", TwistMsg, velCmdCallback) #
rospy.Subscriber("path_seg", PathSegmentMsg, pathSegmentCallback)
rospy.Subscriber("map_pos", PoseStampedMsg, poseCallback)
naptime = rospy.Rate(RATE)
vel_cmd = TwistMsg()
point = PointMsg()
# while(not ros.Time.isValid()):
#pass
print "Entering main loop"
while not rospy.is_shutdown():
if(currSeg is not None):
# print "seg type %i " % currSeg.seg_type
# print "ref_point %s " % currSeg.ref_point
# print "curv %f" % currSeg.curvature
print "dist done %f" % currState.segDistDone
if stopped:
stopForEstop(desVelPub,segStatusPub)
continue
if(currSeg is not None):
# Eventually this should work with arcs, spins and lines, but right
# now its only working with lines
if(currSeg.seg_type == PathSegmentMsg.LINE):
# if there is an obstacle and the obstacle is within the segment length
print ping_angle
if(obsExists and obsDist/currSeg.seg_length < 1.0-currState.segDistDone and ping_angle > 60 and ping_angle < 140):
stopForObs(desVelPub,segStatusPub)
continue
vel_cmd.linear.x = lastVCmd
vel_cmd.angular.z = lastOCmd
point.x = pose.pose.position.x
point.y = pose.pose.position.y
point.z = pose.pose.position.z
currState.updateState(vel_cmd, point, State.getYaw(pose.pose.orientation)) # update where the robot is at
(sVAccel, sVDecel, sWAccel, sWDecel) = computeTrajectory(currSeg,nextSeg) # figure out the switching points in the trajectory
#print "sVAccel: %f sVDecel: %f" % (sVAccel,sVDecel)
#print "sWAccel: %f, sWDecel: %f" % (sWAccel,sWDecel)
des_vel = getVelCmd(sVAccel, sVDecel, sWAccel, sWDecel) # figure out the robot commands given the current state and the desired trajectory
desVelPub.publish(des_vel) # publish the commands
publishSegStatus(segStatusPub) # let everyone else know the status of the segment
# see if its time to switch segments yet
if(currState.segDistDone > 1.0):
print "Finished segment type %i" % (currState.pathSeg.seg_type)
print "currState.segDistDone %f" % (currState.segDistDone)
currSeg = None
else:
# try and get new segments
if(nextSeg is not None):
currSeg = nextSeg # move the nextSegment up in line
nextSeg = None # assume no segment until code below is run
else: # didn't have a next segment before
try: # so try and get a new one from the queue
currSeg = segments.get(False)
except QueueEmpty: # if the queue is still empty then
currSeg = None # just set it to None
try: # see if a next segment is specified
nextSeg = segments.get(False) # try and get it
except QueueEmpty: # if nothing specified
nextSeg = None # set to None
point = PointMsg()
point.x = pose.pose.position.x
point.y = pose.pose.position.y
point.z = pose.pose.position.z
currState.newPathSegment(currSeg, point, pose.pose.orientation)
des_vel = TwistMsg()
desVelPub.publish(des_vel) # publish all zeros for the des_vel
publishSegStatus(segStatusPub) # publish that there is no segment
if(currSeg is not None):
rospy.logwarn("Starting a new segment, type %i" %currSeg.seg_type)
rospy.logwarn("\tcurvature: %f" % currSeg.curvature)
rospy.logwarn("\tref_point: %s" % currSeg.ref_point)
naptime.sleep()
continue
if(goalList[i] > len(goalList)+1):
goalPoint.none = True;
goalPoint.point = goalList[i]
def main():
global pose
global goalList
rospy.init_node('goal_planner_alpha')
rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback)
goalPointPub = rospy.Publisher('goal_point', Goal)
naptime = rospy.Rate(RATE)
endOfObstacles = PointMsg()
endOfObstacles.x = -3.48
endOfObstacles.y = 20.69
goalList.append(endOfObstacles)
needsToTurn = PointMsg()
needsToTurn.x = -2.37
needsToTurn.y = 21.34
goalList.append(needsToTurn)
finalGoalPoint = PointMsg()
finalGoalPoint.x = 5.47
finalGoalPoint.y = 12.04
goalList.append(finalGoalPoint)
def updateState(self, vel_cmd, point, psi):
"""
Integrates along the desired path vector and projects the actual path
vector onto the desired vector
Inputs
------
vel_cmd is expected to be of type Twist
point is expected to be of type Point
Returns
-------
True if everything went okay
False if an error occurred
"""
dt = 1.0/20.0
if(self.pathSeg.seg_type == PathSegmentMsg.LINE):
approx_equal = lambda a,b,t:abs(a-b)<t
# grab the angle of the path
angle = State.getYaw(self.pathSeg.init_tan_angle)
# grab the starting point
p0 = self.pathSeg.ref_point
# calculate another point along the line
p1 = PointMsg()
p1.x = p0.x + self.pathSeg.seg_length*cos(angle)
p1.y = p0.y + self.pathSeg.seg_length*sin(angle)
# line segment
tanVecMag = pow(p0.x-p1.x,2) + pow(p0.y-p1.y,2)
# intersection point
u = ((point.x - p0.x)*(p1.x-p0.x) + (point.y-p0.y)*(p1.y-p0.y))/tanVecMag
intersect = PointMsg()
intersect.x = point.x + u*cos(angle)
intersect.y = point.y + u*sin(angle)
d = sqrt(pow(intersect.x-p0.x,2)+pow(intersect.y-p0.y,2))
# see where this point will lead if followed along the line for a distance d
testX = intersect.x + d*cos(angle)
testY = intersect.y + d*sin(angle)
# if the distance gets smaller then the d should be negative
# if the distance gets bigger then the d should be positive
if(sqrt(pow(testX-p0.x,2)+pow(testY-p0.y,2)) < d):
d = -d
self.segDistDone = d/self.pathSeg.seg_length
elif(self.pathSeg.seg_type == PathSegmentMsg.ARC):
self.segDistDone += abs((vel_cmd.angular.z*dt)/(self.pathSeg.seg_length/abs(self.pathSeg.curvature)))
"""
tanAngStart = State.getYaw(self.pathSeg.init_tan_angle)
rho = self.pathSeg.curvature
r = 1/abs(rho)
if(rho >= 0):
startAngle = tanAngStart - pi/2
else:
startAngle = tanAngStart + pi/2
ref_point = self.pathSeg.ref_point
rVec = State.createVector(ref_point, point)
theta = atan2(rVec.y,rVec.x)
phi = startAngle % (2*pi)
if(abs(phi-2*pi) < phi):
phi = phi - 2*pi
posPhi = phi % (2*pi)
posTheta = theta % (2*pi)
# figure out angle halfway between end and start angle
if(rho >= 0):
halfAngle = self.pathSeg.seg_length/(2*r)
finAngle = self.pathSeg.seg_length/r
else:
halfAngle = self.pathSeg.seg_length/(2*r)
finAngle = -self.pathSeg.seg_length/r
# find theta in terms of starting angle
if(rho >= 0):
if(posTheta > posPhi):
beta = posTheta - posPhi
else:
beta = 2*pi-posPhi+posTheta
else:
if(posTheta < posPhi):
beta = posTheta - posPhi
else:
beta = posTheta-posPhi-(2*pi)
# figure out what region the angle is in
if(rho >= 0):
if(beta >= 0 and beta <= halfAngle+pi): # beta is in the specified arc
alpha = beta
else:
alpha = -(2*pi-beta)
else:
if(beta >= halfAngle-pi and beta <= 0):
alpha = beta;
else:
alpha = beta + (2*pi)
if(rho >= 0):
self.segDistDone = r*alpha/self.pathSeg.seg_length
else:
self.segDistDone = -r*alpha/self.pathSeg.seg_length
"""
elif(self.pathSeg.seg_type == PathSegmentMsg.SPIN_IN_PLACE):
rho = self.pathSeg.curvature
phi = State.getYaw(self.pathSeg.init_tan_angle)
posPhi = phi % (2*pi)
posPsi = psi % (2*pi)
if(rho >=0):
halfAngle = self.pathSeg.seg_length/2.0
else:
halfAngle = self.pathSeg.seg_length/2.0-pi
# find beta in terms of starting angle
if(posPsi > posPhi):
beta = posPsi - posPhi
else:
beta = 2*pi-posPhi+posPsi
# figure out what region the angle is in
if(beta >= 0 and beta <= halfAngle+pi): # beta is in the specified arc
alpha = beta
else:
alpha = beta - 2*pi
self.segDistDone = alpha/self.pathSeg.seg_length
else:
pass # should probably throw an unknown segment type error
# update the current velocity
self.v = vel_cmd.linear.x
self.w = vel_cmd.angular.z
# update the current point and heading
self.point= point
self.psi = psi
