def addCell(x, y, topic):
pub = rospy.Publisher(topic, GridCells, queue_size = 10)
global greenCells
global redCells
global blueCells
newCell = Point()
newCell.x = x
newCell.y = y
newCell.z = 0
if topic == "greenCells" :
greenCells.cells.append(newCell)
pub.publish(greenCells)
elif topic == "redCells" :
redCells.cells.append(newCell)
pub.publish(redCells)
elif topic == "blueCells" :
blueCells.cells.append(newCell)
pub.publish(blueCells)
else:
print "***cell topic not defined in addCell()***"
def publishObstacles(self):
"""
Publishes the obstacles as markers
"""
mk = Marker()
mk.header.stamp = rospy.get_rostime()
mk.header.frame_id = '/base_link'
mk.ns='basic_shapes'
mk.id = 0
mk.type = Marker.POINTS
mk.scale.x = 0.3
mk.scale.y = 0.3
mk.scale.z = 0.3
mk.color.r = 1.0
mk.color.a = 1.0
for value in self.obstacle_map.obstacles_in_memory:
p = Point()
p.x = value[0]
p.y = value[1]
mk.points.append(p)
self.obs_pub.publish(mk)
def cb_points3d(self, p):
for i in range(len(p.ids)):
mp = Point()
mp.x = p.x[i]
mp.y = p.y[i]
mp.z = p.z[i]
self.tracks3d[p.ids[i]].append( mp )
def pubRviz(self, pos):
msgs = MarkerArray()
for p in range(len(pos)):
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'marker'
msg.action = 0
msg.id = p
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
if p == 0:
msg.color.r = 1.0
msg.color.a = 1.0
else:
msg.color.r = 1.0
msg.color.g = 1.0
msg.color.a = 1.0
for i in range(len(pos[p])):
point = Point()
point.x = pos[p][i][0]
point.y = pos[p][i][1]
point.z = pos[p][i][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
self.mpub.publish(msgs)
def getWorldPointFromIndex(index): point=Point() #print "GetX: %i" % getX(index) point.x=(getX(index)*resolution)+offsetX + (.5 * resolution) point.y=(getY(index)*resolution)+offsetY + (.5 * resolution) point.z=0 return point
def prepare_and_publish_geometry_msg(self):
'''
Fill and publish point message
'''
point_msg = Point()
point_msg.x = 1.0;
point_msg.y = 1.0;
point_msg.z = 1.0;
self.point_msg_pub.publish(point_msg)
'''
Fill and publish 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;
self.pointstamped_msg_pub.publish(point_stamped_msg)
'''
Fill and publish 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;
self.twist_msg_pub.publish(twist_msg)
'''
Fill and publish pose 2D message
Please, do it your self for practice
'''
pose_2d_msg = Pose2D()
'''
Fill and publish pose stamped message
Please, do it your self for practice
'''
pose_stamped_msg = PoseStamped()
'''
Fill and publish transform message
Please, do it your self for practice
'''
transform_msg = Transform()
def __make_mesh(self, name, pose, filename):
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]
point.y = vertex[1]
point.z = vertex[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def get_nearest_cloud(self, msg):
points = list()
# Get all the points in the visible cloud (may be prefiltered by other nodes)
for point in point_cloud2.read_points(msg, skip_nans=True):
points.append(point[:3])
# Convert to a numpy array
points_arr = np.float32([p for p in points]).reshape(-1, 1, 3)
# Compute the COG
cog = np.mean(points_arr, 0)
# Abort if we get an NaN in any component
if np.isnan(np.sum(cog)):
return
# Store the COG in a ROS Point object
cog_point = Point()
cog_point.x = cog[0][0]
cog_point.y = cog[0][1]
cog_point.z = cog[0][2]
# Give the COG a unit orientation and store as a PoseStamped message
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = msg.header.frame_id
target.pose.position = cog_point
target.pose.orientation.w = 1.0
# Publish the PoseStamped message on the /target_pose topic
self.target_pub.publish(target)
def drawlandmark(pub):
global pt_count
points_tag = Marker()
points_tag.header.frame_id = "/base_link"
points_tag.action = Marker.ADD
points_tag.header.stamp = rospy.Time.now()
points_tag.lifetime = rospy.Time(0)
points_tag.scale.x = 0.1;
points_tag.scale.y = 0.1;
points_tag.scale.z = 0.1;
points_tag.color.a = 1.0;
points_tag.color.r = 1.0;
points_tag.color.g = 0.0;
points_tag.color.b = 0.0;
points_tag.ns = "pts_line"
pt_count+=1
points_tag.id = pt_count
points_tag.type = Marker.POINTS
for x in range(6):
p1 = Point()
p1.x = tagnum_x[x]/100
p1.y = tagnum_y[x]/100
p1.z = 0
points_tag.points.append(p1)
pub.publish(points_tag)
def mainControlLoop():
global panTilt
panTilt = PanTilt('/dev/ttyUSB0')
panTilt.setSpeed(1000)
panTilt.centerCamera()
cameraAim = Point()
cameraAim.z = 0
random.seed()
# blobTracker = rospy.Subscriber('', Point, callback)
trackingCamera = rospy.Publisher('trackingCamera', Point)
while not rospy.is_shutdown():
randomPan = random.randint(-10, 10)
randomTilt = random.randint(-10, 10)
panTilt.aimCamera(randomPan, randomTilt)
cameraAim.x, cameraAim.y = panTilt.getCameraAim()
print cameraAim.x, cameraAim.y
trackingCamera.publish(cameraAim)
rospy.sleep(0.5)
return
def publish_cell(x, y, state):
"""
Creates and adds a cell-location to its corresponding list to be published in the near future
:param x: The X position (in terms of cell number) of the cell to color.
:param y: The Y position (in terms of cell number) of the cell to color.
:param state: The cell "state" to fill in, either expanded, wall, path, or frontier
"""
db_print('publishCell')
global expanded_cells, frontier_cells, wall_cells, path_cells
p = Point()
p.x, p.y = map_to_world(x, y)
# p.x -= CELL_WIDTH / 2
# p.y -= CELL_HEIGHT / 2
p.z = 0
if state == 'expanded':
expanded_cells.append(p)
elif state == 'wall':
wall_cells.append(p)
elif state == 'path':
path_cells.append(p)
elif state == 'frontier':
frontier_cells.append(p)
else:
print 'Bad state'
def publish(self):
pt = Point()
pt.x = self.newx[0]
pt.y = self.newx[1]
pt.z = self.newx[2]
self.pub.publish(pt)
rospy.logdebug("x : %s dx : %s newx : %s" % (str(self.x), str(self.dx), str(self.newx)))
def update_viz(self):
menu_control = InteractiveMarkerControl()
menu_control.interaction_mode = InteractiveMarkerControl.BUTTON
menu_control.always_visible = True
frame_id = 'base_link'
pose = self.ee_pose
menu_control = self._add_gripper_marker(menu_control)
text_pos = Point()
text_pos.x = pose.position.x
text_pos.y = pose.position.y
text_pos.z = pose.position.z + 0.1
text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z)
menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=0, scale=Vector3(0, 0, 0.03),
text=text,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id=frame_id),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker = InteractiveMarker()
int_marker.name = 'ik_target_marker'
int_marker.header.frame_id = frame_id
int_marker.pose = pose
int_marker.scale = 0.2
self._add_6dof_marker(int_marker)
int_marker.controls.append(menu_control)
self._im_server.insert(int_marker, self.marker_clicked_cb)
def publishGridCells():
global cellOriginX
global cellOriginY
unexploredGridCells = createGridCellsMessage()
frontierGridCells = createGridCellsMessage()
expandedGridCells = createGridCellsMessage()
cellOriginX = originalGridMessage.info.origin.position.x + originalGridMessage.info.resolution/2
cellOriginY = originalGridMessage.info.origin.position.y + originalGridMessage.info.resolution/2
for i in range(0, originalGridMessage.info.height):
for j in range(0, originalGridMessage.info.width):
point = Point()
point.x = cellOriginX + originalGridMessage.info.resolution*j
point.y = cellOriginY + originalGridMessage.info.resolution*i
tempCellCoordinate = CellCoordinate(j, i)
if grid[tempCellCoordinate].type == CellType.Unexplored:
unexploredGridCells.cells.append(point)
elif grid[tempCellCoordinate].type == CellType.Frontier:
frontierGridCells.cells.append(point)
elif grid[tempCellCoordinate].type == CellType.Expanded:
expandedGridCells.cells.append(point)
unexplored_cell_pub.publish(unexploredGridCells)
frontier_cell_pub.publish(frontierGridCells)
expanded_cell_pub.publish(expandedGridCells)
def getPoint(gridpos, resolution, offsetX, offsetY):
point = Point()
point.x = (gridpos[0] * resolution) + offsetX + (.5 * resolution)
point.y = (gridpos[1] * resolution) + offsetY + (.5 * resolution)
point.z = 0
return point
def find_color(self, im, label_color, mask):
contours = cv2.findContours(mask, cv2.cv.CV_RETR_TREE, cv2.cv.CV_CHAIN_APPROX_SIMPLE)[0]
#cv2.drawContours(im, contours, -1, (255, 255, 255), 2)
approx_contours = []
for c in contours:
area = cv2.contourArea(c)
if area < 100: continue
perim = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, .05*perim, True)
#if len(approx) == 4 and len(cv2.convexityDefects(c, cv2.convexHull(c))) <= 1:
if len(approx) == 4:
approx_contours.append(approx)
moments = cv2.moments(c)
center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00']))
cv2.circle(im, center, 3, (255, 100, 100), 4)
print "Moment: ({}, {})".format(center[0], center[1])
msg_color = ColorRGBA()
msg_color.r, msg_color.g, msg_color.b = label_color
self.msg.colors.append(msg_color)
print "Label color: {}".format(label_color)
msg_size = Float64()
#msg_size.data = max(math.sqrt((approx[1][0][0]-approx[0][0][0])**2+(approx[1][0][1]-approx[0][0][1])**2), math.sqrt((approx[2][0][0]-approx[1][0][0])**2+(approx[2][0][1]-approx[2][0][0])**2))
msg_size.data = float((max(approx, key=lambda x: x[0][0])[0][0] - min(approx, key=lambda x: x[0][0])[0][0])) / len(im[0])
print "Width: {}".format(msg_size.data)
self.msg.sizes.append(msg_size)
msg_loc = Point()
msg_loc.x, msg_loc.y = float(center[0]) / len(im[0]), float(center[1]) / len(im)
self.msg.locations.append(msg_loc)
def global2roomba(): dx = pc.points[2].x-pc.points[0].x dy = pc.points[2].y-pc.points[0].y dd = np.sqrt(dx*dx + dy*dy ) theta_ = 0.0 if dx==0: if dy>0: theta_ = pi_2 elif dy<0: theta_ = -pi_2 elif dx>0: theta_ = np.arctan(dy/dx) elif dx<0: theta_ = np.arctan(dy/dx) + pi r_pt = Point() quaternion = ( roomba_position.orientation.x, roomba_position.orientation.y, roomba_position.orientation.z, roomba_position.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) alpha = euler[2] beta = theta_-alpha #r_pt.x = random_r*np.cos(beta) #r_pt.y = random_r*np.sin(beta) ####r_pt.x = dd*np.cos(beta) ####r_pt.y = dd*np.sin(beta) r_pt.x = dd; # (r, theta), r r_pt.y = beta; # (r, theta), theta return r_pt
def cnt_all_obj_inside_roi(self, roiT):
rois = self.get_rois(roiT)
objT_cnt = dict()
for k, v in self._obj.iteritems():
for obj in self._obj[k]:
if k in rois:
for roi_id in rois[k]:
poly = self._soma_utils[k]._get_polygon(roi_id)
point = Point()
point.x = obj[0].pose.position.x
point.y = obj[0].pose.position.y
if self._soma_utils[k]._inside(point, poly):
#print k, obj[0].id, obj[0].type
if obj[0].type not in objT_cnt:
objT_cnt[obj[0].type] = 1
else:
objT_cnt[obj[0].type] += 1
return objT_cnt
def __init__(self, nodes) :
self.node_zone = MarkerArray()
for node in nodes.nodes :
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.LINE_STRIP
marker.scale.x = 0.1
marker.color.a = 0.8
marker.color.r = 0.7
marker.color.g = 0.1
marker.color.b = 0.2
node._get_coords()
count=0
for i in node.vertices :
#print i[0], i[1]
Vert = Point()
Vert.z = 0.05
Vert.x = node.px + i[0]
Vert.y = node.py + i[1]
marker.points.append(Vert)
#vertname = "%s-%d" %(node.name, count)
Pos = Pose()
Pos.position = Vert
# OJO: esto hay que hacerlo de alguna forma
#self._vertex_marker(vertname, Pos, vertname)
count+=1
marker.points.append(marker.points[0])
self.node_zone.markers.append(marker)
idn = 0
for m in self.node_zone.markers:
m.id = idn
idn += 1
def laser_cb(self,data):
self.now=rospy.get_rostime()
if self.feedback=='recieved':
self.feedback=''
#rospy.loginfo('sleeping')
rospy.sleep(0.5)
if data.header.stamp.secs==self.now.secs:
#print 'time qt', data.header.stamp.secs==self.now.secs
LaserData=[]
count=0
for i in data.ranges:
#print 'get total points:', count
if 0.0<i<0.8:
if data.angle_min+data.angle_increment*count<=data.angle_max+data.angle_increment:
point=Point()
angle=data.angle_min+data.angle_increment*count+self.oriation_angle
point.x=i*numpy.cos(angle)+self.pose.position.x-0.1
point.y=i*numpy.sin(angle)+self.pose.position.y+0.05
#print 'i:', i, type(point.x), point.x ,type(numpy.nan) ,point.x==numpy.nan
LaserData.append(point)
else:
rospy.loginfo( 'out of range' )
else:
pass
count+=1
if self.check(LaserData): #判断是否在误差许可之内为地图上已知点
rospy.loginfo( 'obstacle detected' )
self.stop_flag.publish('stop')
else:
#print False
pass
self.LaserDataMarker(LaserData)####################visual_test
def docking(self,msg):
if self.start is False:
return
#go to amussel
print("Picking up amussel with ID " + msg.header.frame_id)
heading = [msg.position.north - self.position.north, msg.position.east - self.position.east]
dist = sqrt(pow(heading[0], 2) + pow(heading[1], 2))
p = Point()
p.x = self.position.north + (dist - 0.05) * heading[0] / dist
p.y = self.position.east + (dist - 0.05) * heading[1] / dist
self.cl.send_position_goal(p)
self.cl.send_perch_goal(msg.header.frame_id)
if (dist<-10):
# perch
self.cl.send_perch_goal(msg.header.frame_id)
# go to position (with amussel docked)
p = Point(0,0,0)
self.cl.send_position_goal(p)
# release amussel
self.cl.send_release_goal()
# go to another position
p = Point(-10,-10,0)
self.cl.send_position_goal(p)
def publish(self):
marker = ImageMarker2()
now = rospy.Time.now()
marker.id = self.marker_id
marker.header.stamp = now
marker.header.frame_id = self.frame_id
marker.type = ImageMarker2.POLYGON3D
point_array = PointArrayStamped()
point_array.header.frame_id = self.frame_id
point_array.header.stamp = now
for i in range(self.RESOLUTION + 1) + [0]:
theta = 2 * math.pi / self.RESOLUTION * i
x = self.radius * math.cos(theta)
y = self.radius * math.sin(theta)
point = Point()
point.x = x
point.y = y
point.z = 0
point_array.points.append(point)
marker.points3D = point_array
if self.color:
marker.outline_colors = [self.color]
if self.fill:
marker.filled = 1
else:
marker.filled = 0
marker.fill_color = self.color
self.publisher.publish(marker)
def _create_add_line(self, points, edges, point_group_name, id, red, green, blue):
all_points = []
for i, end_points in enumerate(edges):
for endpoint_index in end_points:
pt1, pt2 = Point(), Point()
pt1.x, pt1.y, pt1.z = points[i][0], points[i][1], points[i][2]
all_points.append(pt1)
pt2.x, pt2.y, pt2.z = points[endpoint_index][0], points[endpoint_index][1], points[endpoint_index][2]
all_points.append(pt2)
marker = Marker()
marker.header.frame_id = "odom_combined"
marker.header.stamp = rospy.get_rostime()
marker.ns = point_group_name
marker.id = id
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.points = all_points
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.003
marker.color.r = red
marker.color.g = green
marker.color.b = blue
marker.color.a = 1.0
marker.lifetime = rospy.Duration()
return marker
def _get_surface_marker(pose, dimensions):
''' Function that generates a surface marker'''
int_marker = InteractiveMarker()
int_marker.name = 'surface'
int_marker.header.frame_id = 'base_link'
int_marker.pose = pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=2000,
lifetime=rospy.Duration(2),
scale=dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.8, 0.0, 0.4, 0.4),
pose=pose)
button_control.markers.append(object_marker)
text_pos = Point()
position = pose.position
dimensions = dimensions
text_pos.x = position.x + dimensions.x / 2 - 0.06
text_pos.y = position.y - dimensions.y / 2 + 0.06
text_pos.z = position.z + dimensions.z / 2 + 0.06
text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=2001,
scale=Vector3(0, 0, 0.03), text=int_marker.name,
color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))
button_control.markers.append(text_marker)
int_marker.controls.append(button_control)
return int_marker
def find_color(self, passed_im, label_color, mask):
im = passed_im.copy()
if self.isTesting:
self.image = im
contours = cv2.findContours(mask, cv2.cv.CV_RETR_TREE, cv2.cv.CV_CHAIN_APPROX_SIMPLE)[0]
approx_contours = []
for c in contours:
area = cv2.contourArea(c)
if area < 400:
continue
perim = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, .05*perim, True)
if len(approx) == 4:
approx_contours.append(approx)
self.msg.areas.append(area)
self.msg.colors.append(label_color)
moments = cv2.moments(c)
center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00']))
msg_loc = Point()
msg_loc.x, msg_loc.y = float(center[0]) / len(im[0]), float(center[1]) / len(im)
self.msg.locations.append(msg_loc)
self.msg.heights.append(float((max(approx, key=lambda x: x[0][1])[0][1] - min(approx, key=lambda x: x[0][1])[0][1])) / len(im))
cv2.putText(im, label_color, center, cv2.FONT_HERSHEY_PLAIN, 2, (100, 255, 100))
print "Label color: {}".format(label_color)
if approx_contours:
if self.isTesting:
cv2.drawContours(self.image, approx_contours, -1, (100, 255, 100), 2)
def Draw_GoalPnt(goal_pnts): goal_positions = MarkerArray(); marker = Marker(); for goal in goal_pnts: marker.header.frame_id = "base_link"; marker.type = marker.SPHERE_LIST; marker.action = marker.ADD; marker.scale.x = 0.03; marker.scale.y = 0.03; marker.scale.z = 0.03; marker.pose.orientation.w = 1; marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 showpnt = Point(); showpnt.x = goal.x; showpnt.y = goal.y; showpnt.z = goal.z; marker.points.append(showpnt); marker_publisher.publish(marker);
def _get_object_marker(self, index, mesh=None):
'''Generate a marker for world objects'''
int_marker = InteractiveMarker()
int_marker.name = World.objects[index].get_name()
int_marker.header.frame_id = 'base_link'
int_marker.pose = World.objects[index].object.pose
int_marker.scale = 1
button_control = InteractiveMarkerControl()
button_control.interaction_mode = InteractiveMarkerControl.BUTTON
button_control.always_visible = True
object_marker = Marker(type=Marker.CUBE, id=index,
lifetime=rospy.Duration(2),
scale=World.objects[index].object.dimensions,
header=Header(frame_id='base_link'),
color=ColorRGBA(0.2, 0.8, 0.0, 0.6),
pose=World.objects[index].object.pose)
if (mesh != None):
object_marker = World._get_mesh_marker(object_marker, mesh)
button_control.markers.append(object_marker)
text_pos = Point()
text_pos.x = World.objects[index].object.pose.position.x
text_pos.y = World.objects[index].object.pose.position.y
text_pos.z = (World.objects[index].object.pose.position.z +
World.objects[index].object.dimensions.z / 2 + 0.06)
button_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING,
id=index, scale=Vector3(0, 0, 0.03),
text=int_marker.name, color=ColorRGBA(0.0, 0.0, 0.0, 0.5),
header=Header(frame_id='base_link'),
pose=Pose(text_pos, Quaternion(0, 0, 0, 1))))
int_marker.controls.append(button_control)
return int_marker
def arHandler(self, marker, armname):
pose = marker.pose.pose
if armname == 'left':
tool_frame = ConstantsClass.ToolFrame.Left
elif armname == 'right':
tool_frame = ConstantsClass.ToolFrame.Right
time = self.listener.getLatestCommonTime(ConstantsClass.Camera, tool_frame)
if time == 0:
return
(trans, rot) = self.listener.lookupTransform(ConstantsClass.Camera, tool_frame, time)
if (self.state == ImageDetectionClass.State.CalibrateLeft):
offset = Point()
offset.x = pose.position.x - trans[0]
offset.y = pose.position.y - trans[1]
offset.z = pose.position.z - trans[2]
self.offset = offset
self.state = ImageDetectionClass.State.Calibrated
elif self.state == ImageDetectionClass.State.Calibrated:
pos = Util.positionSubtract(pose.position, self.offset)
gp = PoseStamped()
gp.header.stamp = marker.header.stamp
gp.header.frame_id = marker.header.frame_id
gp.pose.position = pos
gp.pose.orientation = Util.makeQuaternion(rot[3], rot[0], rot[1], rot[2])
self.leftGripperPose = gp
self.debugAr(gp)
def draw_tags(pub):
global pt_count
tag_pos_marker = Marker()
tag_pos_marker.header.frame_id = "/base_link"
tag_pos_marker.header.stamp = rospy.Time.now()
tag_pos_marker.action = Marker.ADD
tag_pos_marker.scale.x = 0.6;
tag_pos_marker.scale.y = 0.6;
tag_pos_marker.scale.z = 0.6;
tag_pos_marker.color.a = 1.0;
tag_pos_marker.color.r = 0.0;
tag_pos_marker.color.g = 0.0;
tag_pos_marker.color.b = 1.0;
tag_pos_marker.ns = "tags"
tag_pos_marker.type = Marker.POINTS
tag_pos_marker.id = pt_count
pt_count +=1
for zz in range(6):
tag_pos_point = Point()
tag_pos_point.x = tag_positions[zz].x/PLOT_DOWNSIZE#100.0
tag_pos_point.y = tag_positions[zz].y/PLOT_DOWNSIZE#100.0
tag_pos_point.z = 0
tag_pos_marker.points.append(tag_pos_point)
#print "Publisginh tags"
pub.publish(tag_pos_marker)
def geohash(self, data, width, height, map_origin): (X,Y)=(1,1) pose_mean=Point() pose_mean.x=(width/2)*self.map.info.resolution*X+map_origin.position.x pose_mean.y=(height/2)*self.map.info.resolution*Y+map_origin.position.y self._map_divide_store(width, height, data, map_origin.position, pose_mean, (X,Y))
def create_pose_stamped(pose, frame_id = 'torso_lift_link'):
m = PoseStamped()
m.header.frame_id = frame_id
m.header.stamp = rospy.Time()
m.pose = Pose(Point(*pose[0:3]), Quaternion(*pose[3:7]))
return m
def main():
rospy.init_node('IK_Control', anonymous=True)
global current_joint_state
rad_from_deg = np.pi/180.
deg_from_rad = 180./np.pi
ik_solver = IK(ARM_BASE_LINK, GRIPPER_LINK)
# ck = camera_forward_kinematics.camerakinematics()
# Describing the publisher subscribers
rospy.Subscriber('/joint_states', JointState, get_joint_state)
arm_pub = rospy.Publisher(ROSTOPIC_SET_ARM_JOINT, JointState, queue_size=1)
pan_pub = rospy.Publisher(ROSTOPIC_SET_PAN_JOINT, Float64, queue_size=1)
tilt_pub = rospy.Publisher(ROSTOPIC_SET_TILT_JOINT, Float64, queue_size=1)
gripper_open_pub = rospy.Publisher(ROSTOPIC_OPEN_GRIPPER, Empty, queue_size=1)
gripper_close_pub = rospy.Publisher(ROSTOPIC_CLOSE_GRIPPER, Empty, queue_size=1)
tf_listener = tf.TransformListener()
rospy.sleep(2)
# Homing of all servos
home_joint = [0.004601942375302315, -0.4218447208404541, 1.6260197162628174, -0.1426602154970169, 0.010737866163253784]
# home_joint = [0.0, 0.0, 1.22, -0.142, 0.0]
set_arm_joint(arm_pub, home_joint)
close_gripper(gripper_close_pub)
set_camera_angles(pan_pub, tilt_pub, rad_from_deg*0., rad_from_deg*20.0) #, ck)
rospy.sleep(10)
while not rospy.is_shutdown():
try:
tf_listener.waitForTransform("/bottom_plate", "/icp_cuboid_frame", rospy.Time.now(), rospy.Duration(4.0))
P, Q = tf_listener.lookupTransform("/bottom_plate", "/icp_cuboid_frame", rospy.Time(0))
print("Box in bottom plate frame")
print(P, Q)
break
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
print(e)
O = tf.transformations.euler_from_quaternion(Q)
Q = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2])
Q1 = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2]+np.pi/2)
poses = [Pose(Point(P[0], P[1], P[2]+0.20), Quaternion(Q[0], Q[1], Q[2], Q[3])),
Pose(Point(P[0], P[1], P[2]+0.15), Quaternion(Q[0], Q[1], Q[2], Q[3])),
Pose(Point(P[0], P[1], P[2]+0.25), Quaternion(Q[0], Q[1], Q[2], Q[3])),
Pose(Point(P[0]-0.1, P[1]-0.2, P[2]+0.15), Quaternion(Q1[0], Q1[1], Q1[2], Q1[3]))]
# Position, Orientation and Dimension
obstacles_cuboids = np.array([[[-0.12, 0, 0.1025], [0, 0, 0], [0.08, 0.19, 0.205]],
[[-0.11, 0, 0.305], [0, 0, 0], [0.07, 0.19, 0.20]],
[[-0.06, 0.0, 0.455], [0, 0, 0], [0.1, 0.15, 0.1]],
[[P[0], P[1], P[2]], [O[0], O[1], O[2]], [0.2, 0.03, 0.1]]])
prm = PRM.ProbabilisticRoadMap(obstacles_cuboids)
# prm.makeGraph(120.0)
target_joint = None
itr = 0
home_arm(arm_pub)
for pose in poses:
print(current_joint_state)
print("Reaching a joint state")
if current_joint_state:
target_joint = compute_ik(
ik_solver, pose, current_joint_state)
if target_joint:
# print(np.array([current_joint_state]))
# print(target_joint)
path = prm.makePlan(np.array([current_joint_state])*deg_from_rad, np.array([target_joint])*deg_from_rad)
# print(len(path))
# for joint_angles in path:
# joint_angles = (joint_angles*rad_from_deg).tolist()
# set_arm_joint(arm_pub, np.array(joint_angles))
# rospy.sleep(5)
set_arm_joint(arm_pub, target_joint)
rospy.sleep(8)
if itr == 0 or itr == 3:
open_gripper(gripper_open_pub)
rospy.sleep(4)
if itr == 1:
close_gripper(gripper_close_pub)
rospy.sleep(4)
# Use the following condition to detect if the object is grasped or not
if(np.abs(current_gripper_state[0]) < MIN_CLOSING_GAP and np.abs(current_gripper_state[1] < MIN_CLOSING_GAP)):
state = "Not grabbed"
rospy.sleep(4)
itr += 1
# raw_input("Robot ready to close gripper. Press Enter to continue."
home_arm(arm_pub)
open_gripper(gripper_open_pub)
set_arm_joint(arm_pub, home_joint)
import rospy
from rospy import Publisher, Subscriber
from rospy import Service, ServiceProxy
#from gs_interfaces.msg import PointGPS
from geometry_msgs.msg import Point
from gs_interfaces.msg import SimpleBatteryState
from swarm_drones.msg import Telemetry
from swarm_drones.srv import VotingResponse, VotingRequest, Voting
from swarm_drones.srv import VotiResponse, VotiRequest, Voti
from swarm_drones.srv import RepeaterRequest, RepeaterResponse, Repeater
from std_msgs.msg import Int32
v = 5
batteryForHome = 10.5
#position = PointGPS()
position = Point()
voit = 0
telOrange = Telemetry()
telBlue = Telemetry()
power = SimpleBatteryState()
com = 0
i_repeater = False
def time(x, y, l, w, v, hr, hw):
def gipotenuza(a, b):
return sum(list(map(lambda x: x * x, (a, b))))**0.5
return ((gipotenuza(l, w) / 2 + hr - hw)) / v, gipotenuza(x, y) / v, (
gipotenuza(l - x, w - y) + hr - hw
) / v # от ретрансятора до дома, от точки до дома, точки до ретранслятора
def setpos(self):
#No idea what value to use!
print("Set position to (0,0)")
self.pub_set_pos.publish(Point(0, 0, 0)) #value in cm
def receive_image(image_data):
global red_center, blue_center, red_pos_mm, blue_pos_mm, red_odometry
global blue_odometry, red_velocity, blue_velocity, red_acceleration
global blue_acceleration, counter
image_array = np.fromstring(image_data.data, np.uint8)
cv2_image = cv2.imdecode(image_array, cv2.IMREAD_COLOR)
# Mask by hue and find center
hsv = cv2.cvtColor(cv2_image, cv2.COLOR_BGR2HSV)
red_lower_1 = np.array([0, 50, 50])
red_upper_1 = np.array([int(1.0 * 180. / 6.), 255, 255])
red_mask_1 = cv2.inRange(hsv, red_lower_1, red_upper_1)
red_lower_2 = np.array([int(5.5 * 180. / 6.), 50, 50])
red_upper_2 = np.array([255, 255, 255])
red_mask_2 = cv2.inRange(hsv, red_lower_2, red_upper_2)
red_mask = red_mask_1 + red_mask_2
red_mask = cv2.erode(red_mask, None, iterations=2)
red_mask = cv2.dilate(red_mask, None, iterations=2)
blue_lower = np.array([int(3.0 * 180. / 6.), 50, 50])
blue_upper = np.array([int(4.5 * 180. / 6.), 255, 255])
blue_mask = cv2.inRange(hsv, blue_lower, blue_upper)
blue_mask = cv2.erode(blue_mask, None, iterations=2)
blue_mask = cv2.dilate(blue_mask, None, iterations=2)
red_contours = cv2.findContours(red_mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
if len(red_contours) > 0:
red_M = cv2.moments(max(red_contours, key=cv2.contourArea))
red_center = Point(int(red_M['m10'] / red_M['m00']),
int(red_M['m01'] / red_M['m00']), 0)
blue_contours = cv2.findContours(blue_mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
if len(blue_contours) > 0:
blue_M = cv2.moments(max(blue_contours, key=cv2.contourArea))
blue_center = Point(int(blue_M['m10'] / blue_M['m00']),
int(blue_M['m01'] / blue_M['m00']), 0)
# Store previous positions for calculating velocity
# and velocities for calculating accerlation
red_pos_mm_old = copy.deepcopy(red_pos_mm)
blue_pos_mm_old = copy.deepcopy(blue_pos_mm)
red_vel_old = copy.deepcopy(red_velocity)
blue_vel_old = copy.deepcopy(blue_velocity)
# Convert camera pixel coordinates to millimeters.
# Position (0, 0, 0) is at the center of the arena.
red_pos_mm.header.stamp = rospy.Time.now()
red_pos_mm.point = Point((red_center.x - 959) / 0.771,
-1 * (red_center.y - 538) / 0.771,
0) # Flip y-axis
blue_pos_mm.header.stamp = rospy.Time.now()
blue_pos_mm.point = Point((blue_center.x - 959) / 0.771,
-1 * (blue_center.y - 538) / 0.771,
0) # Flip y-axis
# Calculate current deltas from starting positions, in millimeters
# In the simulation this is simply the distance from their home base
red_odometry.header.stamp = rospy.Time.now()
# red_odometry.point = Point(red_pos_mm.point.x - red_base_mm.x,
# red_pos_mm.point.y - red_base_mm.y, 0)
red_odometry.point = red_pos_mm.point
blue_odometry.header.stamp = rospy.Time.now()
# blue_odometry.point = Point(blue_pos_mm.point.x - blue_base_mm.x,
# blue_pos_mm.point.y - blue_base_mm.y, 0)
blue_odometry.point = blue_pos_mm.point
red_velocity.header.stamp = rospy.Time.now()
delta_t = ((red_pos_mm.header.stamp.secs +
red_pos_mm.header.stamp.nsecs / 1000000000.) -
(red_pos_mm_old.header.stamp.secs +
red_pos_mm_old.header.stamp.nsecs / 1000000000.))
if delta_t == 0:
delta_t = 0.1
red_velocity.point = Point(
(red_pos_mm.point.x - red_pos_mm_old.point.x) / delta_t,
(red_pos_mm.point.y - red_pos_mm_old.point.y) / delta_t,
(red_pos_mm.point.z - red_pos_mm_old.point.z) / delta_t)
blue_velocity.header.stamp = rospy.Time.now()
delta_t = ((blue_pos_mm.header.stamp.secs +
blue_pos_mm.header.stamp.nsecs / 1000000000.) -
(blue_pos_mm_old.header.stamp.secs +
blue_pos_mm_old.header.stamp.nsecs / 1000000000.))
if delta_t == 0:
delta_t = 0.1
blue_velocity.point = Point(
(blue_pos_mm.point.x - blue_pos_mm_old.point.x) / delta_t,
(blue_pos_mm.point.y - blue_pos_mm_old.point.y) / delta_t,
(blue_pos_mm.point.z - blue_pos_mm_old.point.z) / delta_t)
delta_t = ((red_velocity.header.stamp.secs +
red_velocity.header.stamp.nsecs / 1000000000.) -
(red_vel_old.header.stamp.secs +
red_vel_old.header.stamp.nsecs / 1000000000.))
if delta_t == 0:
delta_t = 0.1
red_accel_vector = Point(
(red_velocity.point.x - red_vel_old.point.x) / delta_t,
(red_velocity.point.y - red_vel_old.point.y) / delta_t,
(red_velocity.point.z - red_vel_old.point.z) / delta_t)
delta_t = ((blue_velocity.header.stamp.secs +
blue_velocity.header.stamp.nsecs / 1000000000.) -
(blue_vel_old.header.stamp.secs +
blue_vel_old.header.stamp.nsecs / 1000000000.))
if delta_t == 0:
delta_t = 0.1
blue_accel_vector = Point(
(blue_velocity.point.x - blue_vel_old.point.x) / delta_t,
(blue_velocity.point.y - blue_vel_old.point.y) / delta_t,
(blue_velocity.point.z - blue_vel_old.point.z) / delta_t)
red_acceleration = np.sqrt(red_accel_vector.x**2 + red_accel_vector.y**2 +
red_accel_vector.z**2)
blue_acceleration = np.sqrt(blue_accel_vector.x**2 +
blue_accel_vector.y**2 +
blue_accel_vector.z**2)
counter += 1
# if counter % 5 == 0:
# cv2.imwrite('images/{0:08d}.png'.format(counter), cv2_image)
# cv2.imshow('cv2_image', cv2_image)
# cv2.waitKey(2)
return
import numpy as np
import rospy
import cv2
from std_msgs.msg import Bool, Int16
from sensor_msgs.msg import CompressedImage
from geometry_msgs.msg import Point, PointStamped
# Capture the Flag base configuration
print("Starting Capture the Flag")
red_odometry = PointStamped()
blue_odometry = PointStamped()
red_acceleration = 0
blue_acceleration = 0
red_center = Point()
blue_center = Point()
red_pos_mm = PointStamped()
blue_pos_mm = PointStamped()
red_velocity = PointStamped()
blue_velocity = PointStamped()
red_base = Point(1400, 98, 0)
blue_base = Point(518, 979, 0)
red_base_mm = Point((red_base.x - 959) / 0.771,
-1 * (red_base.y - 538) / 0.771, 0) # Flip y-axis
blue_base_mm = Point((blue_base.x - 959) / 0.771,
-1 * (blue_base.y - 538) / 0.771, 0) # Flip y-axis
red_flag = False
blue_flag = False
red_score = 0
blue_score = 0
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# This is the wrist link not the gripper itself
gripper_frame = 'wrist_roll_link'
# Position and rotation of two "wave end poses"
gripper_poses = [Pose(Point(0.042, 0.384, 1.826),
Quaternion(0.173, -0.693, -0.242, 0.657)),
Pose(Point(0.047, 0.545, 1.822),
Quaternion(-0.274, -0.701, 0.173, 0.635))]
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
while not rospy.is_shutdown():
for pose in gripper_poses:
# Finish building the Pose_stamped message
# If the message stamp is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
plt.title('target y')
plt.legend()
plt.ion()
plt.show()
plt.pause(0.00001)
print("t_now : %f / t_duration %f" % (t_now, t_duration))
while (not rospy.is_shutdown()) and (t_now - t_init) < t_duration:
now = rospy.get_rostime()
t_now = now.secs + now.nsecs * 1e-9
t_eval = t_now - t_init + t_init_ref
goal_point = Point()
goal_point.x = fx(t_eval)
goal_point.y = fy(t_eval)
ref_pub.publish(goal_point)
# print("publish goal_topic [ %f , %f ]" % (goal_point.x,goal_point.y))
#
#
# print("t_now: %f / t_init: %f " % (t_now,t_init))
plt.figure(1)
plt.subplot(211)
plt.plot(t_eval, goal_point.x, 'r*')
try:
plt.plot(np.array(t_true) - t_init + t_init_ref, x_true, 'k')
except:
def collision_state():
global ms, co_svc, root_link_name, root_link_offset, last_collision_status
diagnostic = DiagnosticArray()
now = rospy.Time.now()
diagnostic.header.stamp = now
collision_status = co_svc.getCollisionStatus()
if (now - last_collision_status > rospy.Duration(5.0)):
num_collision_pairs = len(collision_status[1].lines)
rospy.loginfo(
"check %d collision status, %f Hz", num_collision_pairs,
num_collision_pairs / (now - last_collision_status).to_sec())
last_collision_status = now
# check if ther are collision
status = DiagnosticStatus(name='CollisionDetector',
level=DiagnosticStatus.OK,
message="Ok")
#if any(a): # this calls omniORB.any
for collide in collision_status[1].collide:
if collide:
status.level = DiagnosticStatus.ERROR
status.message = "Robots is in collision mode"
status.values.append(
KeyValue(key="Time", value=str(collision_status[1].time)))
status.values.append(
KeyValue(key="Computation Time",
value=str(collision_status[1].computation_time)))
status.values.append(
KeyValue(key="Safe Posture",
value=str(collision_status[1].safe_posture)))
status.values.append(
KeyValue(key="Recover Time",
value=str(collision_status[1].recover_time)))
status.values.append(
KeyValue(key="Loop for check",
value=str(collision_status[1].loop_for_check)))
frame_id = root_link_name # root id
markerArray = MarkerArray()
for line in collision_status[1].lines:
p1 = Point(*(numpy.dot(root_link_offset[3, 3], line[0]) +
root_link_offset[0:3, 3]))
p2 = Point(*(numpy.dot(root_link_offset[3, 3], line[1]) +
root_link_offset[0:3, 3]))
sphere_color = ColorRGBA(0, 1, 0, 1)
line_width = 0.01
line_length = numpy.linalg.norm(
numpy.array((p1.x, p1.y, p1.z)) - numpy.array((p2.x, p2.y, p2.z)))
sphere_scale = 0.02
# color changes between 0.15(green) -> 0.05(red), under 0.05, it always red
if (line_length < 0.15):
if (line_length < 0.05):
sphere_color = ColorRGBA(1, 0, 0, 1)
else:
ratio = 1.0 - (line_length -
0.05) * 10 # 0.0 (0.15) -> 1.0 ( 0.05)
sphere_scale = 0.02 + ratio * 0.08
sphere_color = ColorRGBA(ratio, 1 - ratio, 0, 1)
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.LINE_LIST
marker.action = marker.ADD
marker.color = sphere_color
marker.points = [p1, p2]
marker.scale.x = line_width
markerArray.markers.append(marker)
sphere_scale = Vector3(sphere_scale, sphere_scale, sphere_scale)
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale = sphere_scale
marker.color = sphere_color
marker.pose.orientation.w = 1.0
marker.pose.position = p1
markerArray.markers.append(marker)
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale = sphere_scale
marker.color = sphere_color
marker.pose.orientation.w = 1.0
marker.pose.position = p2
markerArray.markers.append(marker)
id = 0
for m in markerArray.markers:
m.lifetime = rospy.Duration(1.0)
m.id = id
id += 1
pub_collision.publish(markerArray)
diagnostic.status.append(status)
pub_diagnostics.publish(diagnostic)
def add_point(p):
pt = Point()
pt.x = p[2]
pt.y = p[3]
pt.z = 0.
marker.points.append(pt)
def save_program(self, program_name, frame_id, append=True):
if not self._curr_markers:
print "No ar markers available"
return
print "Saving next position for program {} in {}".format(
program_name, frame_id)
# need to grab the program as is
curr_program = self._programs.get(program_name)
if curr_program is None:
rospy.logerr("This shouldn't happen if create has been called")
return
# TODO: Complete this
new_pose = PoseStamped()
if frame_id in ID_TO_TAGNAME:
# we know the user is trying to define a pose relative to a tag
# we need to do some transformation stuff
# need to grab the marker from self._curr_markers that matches frame_id
marker_id = ID_TO_TAGNAME[frame_id]
curr_marker = None
for marker in self._curr_markers:
if marker.id == marker_id:
curr_marker = marker
break
if curr_marker is None:
rospy.logerr(
'No marker found with frame_id {} and marker_id {} in program {}'
.format(frame_id, marker_id, program_name))
return
else:
# now we have the marker (curr_marker) which has some pose and we can get the robots current position for its arm
# we have to then transform that current position to be relative to the curr_marker.pose
# get position and orientation of the gripper in the base link
position, orientation = self._tf_listener.lookupTransform(
'/base_link', '/gripper_link', rospy.Time(0))
base_link_T_wrist = Pose(Point(*position),
Quaternion(*orientation))
# The transformation from the base_link to this tags frame is just the tags pose
base_link_T_tags = copy.deepcopy(marker.pose)
tags_T_wrist = fetch_api.transform(
fetch_api.inverse_pose(base_link_T_tags.pose),
base_link_T_wrist)
new_pose = PoseStamped(pose=tags_T_wrist)
new_pose.header.frame_id = frame_id
else:
# user is trying to define frame_id in some arbitrary frame
# assume base_link for now
if frame_id == 'base_link':
position, orientation = self._tf_listener.lookupTransform(
'/base_link', '/gripper_link', rospy.Time(0))
base_link_T_wrist = Pose(Point(*position),
Quaternion(*orientation))
new_pose = PoseStamped(pose=base_link_T_wrist)
new_pose.header.frame_id = frame_id
else:
print 'Please use base_link'
return
step = ProgramStep(new_pose)
step.gripper_state = self._gripper.state()
curr_program.add_step(step, append)
self._write_out_programs()
def update_object_pose(self):
""" Function to externally update an object pose."""
# Look down at the table.
rospy.loginfo('Head attempting to look at table.')
Response.force_gaze_action(GazeGoal.LOOK_DOWN)
while (Response.gaze_client.get_state() == GoalStatus.PENDING
or Response.gaze_client.get_state() == GoalStatus.ACTIVE):
rospy.sleep(PAUSE_SECONDS)
if Response.gaze_client.get_state() != GoalStatus.SUCCEEDED:
rospy.logerr('Could not look down to take table snapshot')
return False
rospy.loginfo('Head is now (successfully) staring at table.')
try:
resp = self._segment_tabletop()
rospy.loginfo("Adding landmarks")
self._obj_sides = []
self._reset_objects()
# add the table
xmin = resp.table.x_min
ymin = resp.table.y_min
xmax = resp.table.x_max
ymax = resp.table.y_max
depth = xmax - xmin
width = ymax - ymin
pose = resp.table.pose.pose
pose.position.x = pose.position.x + xmin + depth / 2
pose.position.y = pose.position.y + ymin + width / 2
dimensions = Vector3(depth, width, 0.01)
self._surface = _get_surface_marker(pose, dimensions)
self._im_server.insert(self._surface, self.marker_feedback_cb)
self._im_server.applyChanges()
for cluster in resp.clusters:
points = cluster.points
if (len(points) == 0):
return Point(0, 0, 0)
[minX, maxX, minY, maxY, minZ, maxZ] = [
points[0].x, points[0].x, points[0].y, points[0].y,
points[0].z, points[0].z
]
for pt in points:
minX = min(minX, pt.x)
minY = min(minY, pt.y)
minZ = min(minZ, pt.z)
maxX = max(maxX, pt.x)
maxY = max(maxY, pt.y)
maxZ = max(maxZ, pt.z)
object_sides_list = {
'minX': minX,
'minY': minY,
'minZ': minZ,
'maxX': maxX,
'maxY': maxY,
'maxZ': maxZ
}
self._obj_sides += [object_sides_list]
self._add_new_object(
Pose(
Point((minX + maxX) / 2, (minY + maxY) / 2,
(minZ + maxZ) / 2), Quaternion(0, 0, 0, 1)),
Point(maxX - minX, maxY - minY, maxZ - minZ), False)
return True
except rospy.ServiceException, e:
print "Call to segmentation service failed: %s" % e
return False
def publish_lookahead(self, lookahead):
msg = Point()
msg.x, msg.y = lookahead[:2]
msg.z = 0
self.pub_lookahead.publish(msg)
def __init__(self):
# Give the node a name
# 节点名字
rospy.init_node('calibrate_linear', anonymous=False)
# Set rospy to execute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)
# How fast will we check the odometry values?
# 获取里程计数据的频率
self.rate = rospy.get_param('~rate', 20)
r = rospy.Rate(self.rate)
# Set the distance to travel
# 设置校准的相关参数
self.test_distance = rospy.get_param('~test_distance', 1.0) # meters
self.speed = rospy.get_param('~speed', 0.15) # meters per second
self.tolerance = rospy.get_param('~tolerance', 0.01) # meters
self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0)
self.start_test = rospy.get_param('~start_test', True)
# Publisher to control the robot's speed
# 发布cmd_vel话题
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# Fire up the dynamic_reconfigure server
dyn_server = Server(CalibrateLinearConfig, self.dynamic_reconfigure_callback)
# Connect to the dynamic_reconfigure server
dyn_client = dynamic_reconfigure.client.Client("calibrate_linear", timeout=60)
# The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
# 获取机器人坐标系名字
self.base_frame = rospy.get_param('~base_frame', '/base_footprint')
# The odom frame is usually just /odom
# 获取里程计坐标系名字
self.odom_frame = rospy.get_param('~odom_frame', '/odom')
# Initialize the tf listener
# 初始化tf变换数据的收听功能
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Make sure we see the odom and base frames
# self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))
# rospy.loginfo("Bring up rqt_reconfigure to control the test.")
# self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))
# self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(60.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
rospy.signal_shutdown("tf Exception")
rospy.loginfo("Bring up rqt_reconfigure to control the test.")
self.position = Point()
# Get the starting position from the tf transform between the odom and base frames
# 设置当前位置为机器人的起点
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
move_cmd = Twist()
# 以一定频率开始循环
while not rospy.is_shutdown():
# Stop the robot by default
move_cmd = Twist()
# 如果测试可以开始
if self.start_test:
# Get the current position from the tf transform between the odom and base frames
# 获得当前机器人坐标系在里程计坐标系下的位置(忽略姿态)
self.position = self.get_position()
# Compute the Euclidean distance from the target point
# 计算当前机器人当前位置和起点之间的欧式距离, 也就是现在车向x轴方向开了多少距离
distance = sqrt(pow((self.position.x - x_start), 2) +
pow((self.position.y - y_start), 2))
# Correct the estimated distance by the correction factor
# 通过乘于校正系数, 对距离进行校正
distance *= self.odom_linear_scale_correction
# How close are we?
# 计算当前已经行驶的距离和目标距离的差值
error = distance - self.test_distance
# Are we close enough?
# 如果距离差值小于容忍值, 那么认为已经到达目的地, 停止测试;
# 如果距离差值大于等于容忍值, 那么认为还没有到达目的地或者过头了, 继续前进或后退;
if not self.start_test or abs(error) < self.tolerance:
self.start_test = False
params = {'start_test': False}
rospy.loginfo(params)
dyn_client.update_configuration(params)
else:
# If not, move in the appropriate direction
move_cmd.linear.x = copysign(self.speed, -1 * error)
else:
# 如果测试还没开始, 则不断获取机器人的当前位置来更新机器人的起点
self.position = self.get_position()
x_start = self.position.x
y_start = self.position.y
# 经过上述的if-else判断, 得到当前的所需速度指令
self.cmd_vel.publish(move_cmd)
# 睡眠一定的时间, 保持设定的频率
r.sleep()
# Stop the robot
# 跳出上述while循环, 发出速度为0的指令
self.cmd_vel.publish(Twist())
def detect(self, rgb_message, depth_message, depth_info_message):
sync_header = rgb_message.header
child_sync_header = deepcopy(sync_header)
child_sync_header.frame_id = self.child_frame_id
parent_sync_header = deepcopy(sync_header)
parent_sync_header.frame_id = self.parent_frame_id
# Get raw image data
rgb_image = ros_numpy.numpify(rgb_message)
depth_image = ros_numpy.numpify(depth_message)
# Get location of objects
bounding_boxes, masks, valid_class_labels = self.detector_2D.get_bounding_boxes(rgb_image[..., ::-1],
score=self.score_thresh)
if bounding_boxes is None:
return
# Create copies of depth maps for synchronisation and later use
depth_map_msg = depth_message
depth_map_msg.header = sync_header
depth_info_msg = depth_info_message
depth_info_msg.header = sync_header
# Get projected points from bounding box
object_annotations, valid_bounding_boxes = self.bbox_projector_to_3D.bbox_to_object_annotation(
depth_image, bounding_boxes, depth_info_msg.P, child_sync_header
)
# Visualise points
if self.plot:
# Get detection map (map of points used for object description calculation)
detection_summary_image, detection_map = BBoxProjector.visualise_detection(rgb_image, valid_bounding_boxes,
self.detector_2D.CLASSES,
self.detector_2D.COLOURS)
detection_summary_msg = ros_numpy.msgify(Image, detection_summary_image, encoding="rgb8")
detection_summary_msg.header = child_sync_header
self.detection_summary.publish(detection_summary_msg)
centroids = [[getattr(d.description, s) for s in ['x', 'y', 'z', 'class_id']] for d in object_annotations]
self.marker_publisher.visualise_points(centroids, child_sync_header)
detection_map_msg = ros_numpy.msgify(Image, detection_map, encoding="rgb8")
detection_map_msg.header = sync_header
self.detection_map_pub.publish(detection_map_msg)
# Publish detection messages
labelled_message = LabelledImage(header=sync_header, parent_frame_id=self.parent_frame_id,
image=rgb_message, annotations=object_annotations,
class_labels=valid_class_labels)
self.labelled_rgb_pub.publish(labelled_message)
# Publish depth version for later analysis of depth map
labelled_message.image = depth_map_msg
self.labelled_depth_pub.publish(labelled_message)
# Publish object description as PoseArray message
poses = [Pose(position=Point(d.description.x, d.description.y, d.description.z), orientation=Quaternion(w=1))
for d in object_annotations]
pose_array_msg = PoseArray(header=parent_sync_header, poses=poses)
self.pose_array_pub.publish(pose_array_msg)
# Synchronised messages
self.depth_map_pub.publish(depth_map_msg)
self.depth_info_pub.publish(depth_info_msg)
positive_marker.color.b = 0.0
negative_marker = Marker()
negative_marker.type = Marker.SPHERE_LIST
negative_marker.ns = 'negative_readings'
negative_marker.action = Marker.ADD
negative_marker.header.stamp = rospy.Time(0)
negative_marker.header.frame_id = 'map'
negative_marker.pose.orientation.w = 1.0
negative_marker.scale.x = 0.04
negative_marker.scale.y = 0.04
negative_marker.scale.z = 0.04
negative_marker.color.a = 1.0
negative_marker.color.r = 1.0
negative_marker.color.g = 0.0
negative_marker.color.b = 0.0
for (x, y, z, got_return) in data:
if got_return:
print x, y, z
positive_marker.points.append(Point(x, y, z))
else:
print 'neg', x, y, z
negative_marker.points.append(Point(x, y, z))
# publish the markers
r = rospy.Rate(10)
while not rospy.is_shutdown():
marker_pub.publish(positive_marker)
marker_pub.publish(negative_marker)
r.sleep()
def test(self):
print "Testing..."
rospy.sleep(1)
#### TEST STATE GRABBING ####
print "Left Current Pose:"
print self.curr_state[0]
print "Right Current Pose:"
print self.curr_state[1]
#### TEST FORCE STATE GRABBING ####
print "Current Force Wrench:"
print self.ft_wrench
print "Current Force Magnitude:"
print self.ft_mag
#### TEST LEFT ARM GOAL SENDING ####
l_pose = PoseStamped()
l_pose.header.frame_id = 'torso_lift_link'
l_pose.pose.position = Point(0.6, 0.3, 0.1)
l_pose.pose.orientation = Quaternion(1, 0, 0, 0)
raw_input("send left arm goal")
self.goal_pub[0].publish(l_pose)
#### TEST RIGHT ARM GOAL SENDING
#r_pose = PoseStamped()
#r_pose.header.frame_id = 'torso_lift_link'
#r_pose.pose.position = Point(0.6, -0.3, 0.1)
#r_pose.pose.orientation = Quaternion(1,0,0,0)
#raw_input("send right arm goal")
#self.goal_pub[1].publish(r_pose)
#### TEST POSE SETTING ####
# raw_input("Left Elbow Up")
# self.send_posture('elbowup',0)
# raw_input("Right Elbow Up")
# self.send_posture('elbowup',1)
# raw_input("Left Elbow Down")
# self.send_posture('elbowdown',0)
#raw_input("Right Elbow Down")
#self.send_posture('elbowdown',1)
#raw_input("Both Postures Off")
#self.send_posture(arm=0)
#self.send_posture(arm=1)
#print "Postures adjusted"
#### TEST TRAJECTORY MOTION ####
l_pose2 = PoseStamped()
l_pose2.header.frame_id = 'torso_lift_link'
l_pose2.pose.position = Point(0.8, 0.3, 0.1)
l_pose2.pose.orientation = Quaternion(1, 0, 0, 0)
raw_input("Left trajectory")
#l_pose2 = self.pose_frame_move(self.curr_state[0], -0.1, arm=0)
traj = self.build_trajectory(l_pose2)
self.send_traj_to_contact(traj)
#r_pose2 = PoseStamped()
#r_pose2.header.frame_id = 'torso_lift_link'
#r_pose2.pose.position = Point(0.8, -0.15, -0.3)
#r_pose2.pose.orientation = Quaternion(0,0.5,0.5,0)
#raw_input("Right trajectory")
#r_pose2 = self.pose_frame_move(self.curr_state[1], -0.1, arm=1)
#traj = self.build_trajectory(r_pose2, arm=1)
#self.send_traj(traj,1)
#### RECONFIRM POSITION ####
print "New Left Pose:"
print self.curr_state[0]
print "New Right Pose:"
print self.curr_state[1]
def sendBallPose():
global bx
p = Pose(Point(bx,0.0,0.0), Quaternion(0.0,0.0,0.0,0.0))
t = Twist(Vector3(0.0,0.0,0.0), Vector3(0.0,0.0,0.0))
obj = LinkState('link',p,t,'world')
setLinkState(obj)
[q_rotation.x, q_rotation.y, q_rotation.z, q_rotation.w])
rospy.init_node("speed_controller")
subscriber = rospy.Subscriber("/odometry/filtered", Odometry, newOdom)
publisher = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
twist = Twist()
r = rospy.Rate(10)
xgoal = input("Enter goal for x: ")
ygoal = input("Enter goal for y: ")
goal = Point()
goal.x = xgoal #x goal
goal.y = ygoal #y goal
while not rospy.is_shutdown():
dist_x = goal.x - x
dist_y = goal.y - y
measured_angle = atan2(dist_y, dist_x)
if (abs(measured_angle - theta) > 0.1):
twist.linear.x = 0.0
twist.angular.z = 0.5
print("linear x: ", twist.linear.x, " angular z: ", twist.angular.z)
def ls(self, data):
marker1 = Marker()
marker1.header.frame_id = "/map"
marker1.header.stamp = rospy.Time.now()
marker1.type = marker1.SPHERE
marker1.action = marker1.ADD
marker1.scale.x = 0.05
marker1.scale.y = 0.05
marker1.scale.z = 0.05
if data.header.frame_id == "ls_pos_calc":
marker1.ns = "LS_calc_position"
marker1.id = 11
elif data.header.frame_id == "ls_pos_actu":
marker1.ns = "LS_actu_position"
marker1.id = 12
marker1.color.a = 1.0
marker1.color.r = 0.0
marker1.color.g = 0.0
marker1.color.b = 1.0
marker1.lifetime = rospy.Duration()
marker1.pose.orientation.x = 0.0
marker1.pose.orientation.y = 0.0
marker1.pose.orientation.z = 0.0
marker1.pose.orientation.w = 1.0
marker1.pose.position.x = (data.x_pos) / 1000.0
marker1.pose.position.y = (data.y_pos) / 1000.0
marker1.pose.position.z = (data.z_pos) / 1000.0
#print marker1
self.publisher.publish(marker1)
vector = Point(data.x_pos / 1000.0, data.y_pos / 1000.0,
data.z_pos / 1000.0)
tail = Point(0, 0, 0)
marker = Marker()
marker.header.stamp = rospy.Time(0)
marker.header.frame_id = '/map'
if data.header.frame_id == "ls_pos_calc":
marker.ns = "LS_calc_position"
marker.id = 13
if data.header.frame_id == "ls_pos_actu":
marker.ns = "LS_actu_position"
marker.id = 14
marker.color.g = 0.0
marker.color.b = 1.0
marker.color.r = 1.0
marker.color.a = 0.5
marker.type = marker.ARROW
marker.action = marker.ADD
marker.lifetime = rospy.Duration()
marker.points.append(tail)
marker.points.append(vector)
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.5
self.publisher.publish(marker)
from geometry_msgs.msg import Point
from tf.transformations import quaternion_from_euler
from dynamic_reconfigure.server import Server
from razor_imu_9dof.cfg import imuConfig
# from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
degrees2rad = 0.0175
rospy.init_node("razor_node")
#We only care about the most recent measurement, i.e. queue_size=1
pub = rospy.Publisher('imu', Imu, queue_size=1)
#pub2 = rospy.Publisher('rpy', Point, queue_size=1)
#srv = Server(imuConfig, reconfig_callback) # define dynamic_reconfigure callback
#diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
diag_pub_time = rospy.get_time()
imuMsg = Imu()
rpyMSG = Point()
default_port = '/dev/ttyACM0'
port = rospy.get_param('~port', default_port)
# Check your COM port and baud rate
rospy.loginfo("Opening %s...", port)
try:
ser = serial.Serial(port=port, baudrate=115200, timeout=1)
except serial.serialutil.SerialException:
rospy.logerr("IMU not found at port " + port +
". Did you specify the correct port in the launch file?")
#exit
sys.exit(0)
roll = 0
def msg_from_np_array(self, point_array):
point = Point(point_array[0], point_array[1], point_array[2])
return point
def target_cb(self, data):
self.target = data
try:
cv_image = self.bridge.imgmsg_to_cv2(self.current_image, 'bgr8')
except CvBridgeError as e:
print(e)
image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
image_np = np.asarray(image)
image_np_expanded = np.expand_dims(image_np, axis=0)
image_tensor = detection_graph.get_tensor_by_name('image_tensor:0')
boxes = detection_graph.get_tensor_by_name('detection_boxes:0')
scores = detection_graph.get_tensor_by_name('detection_scores:0')
classes = detection_graph.get_tensor_by_name('detection_classes:0')
num_detections = detection_graph.get_tensor_by_name('num_detections:0')
(boxes, scores, classes, num_detections) = sess.run([boxes, scores, classes, num_detections],
feed_dict={image_tensor: image_np_expanded})
objects=vis_util.visualize_boxes_and_labels_on_image_array(
image,
np.squeeze(boxes),
np.squeeze(classes).astype(np.int32),
np.squeeze(scores),
category_index,
max_boxes_to_draw=2,
min_score_thresh=.7,
use_normalized_coordinates=True,
line_thickness=2)
[image_rows, image_cols, image_ch] = image.shape
target_found = False
center_point = Point()
for i in range(len(objects)):
if(objects[i][0] == 1):
rospy.loginfo("Target found")
target_found = True
[ymin, xmin, ymax, xmax] = objects[i][2]
xmin *= image_cols
xmax *= image_cols
ymin *= image_rows
ymax *= image_rows
center_point.x = int((xmax + xmin)/2)
center_point.y = int((ymax + ymin)/2)
center_point.z = 0
break
img = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB)
image_out = Image()
try:
image_out = self.bridge.cv2_to_imgmsg(img, 'bgr8')
except CvBridgeError as e:
print(e)
image_out.header = self.current_image.header
self.image_pub.publish(image_out)
global attempts, current_attempts
if(target_found):
# publish point
current_attempts = 0
self.point_pub.publish(center_point)
else:
current_attempts += 1
if(current_attempts < attempts):
print("Search target again")
rospy.sleep(0.1)
self.target_repeat_pub.publish(self.target)
else:
print("Target not found")
current_attempts = 0
def gen_point(lower, upper):
p = Point()
p.x = random.uniform(lower, upper)
p.y = random.uniform(lower, upper)
p.z = random.uniform(lower, upper)
return p
x = msg.pose[1].position.x
y = msg.pose[1].position.y
rot_q = msg.pose[1].orientation
(roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
rospy.init_node ('subscriber')
sub = rospy.Subscriber('/gazebo/model_states', ModelStates, callback)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
speed = Twist()
r = rospy.Rate(4)
goal = Point()
goal.x = -2
goal.y = -1
while not rospy.is_shutdown():
inc_x = goal.x - x #distance robot to goal in x
inc_y = goal.y - y #distance robot to goal in y
angle_to_goal = atan2 (inc_y, inc_x) #calculate angle through distance from robot to goal in x and y
print abs(angle_to_goal - theta)
if abs(angle_to_goal - theta) > 0.1: #0.1 because it too exact for a robot if both angles should be exactly 0
speed.linear.x = 0.0
speed.angular.z = 0.3
else:
speed.linear.x = 0.3 #drive towards goal
speed.angular.z = 0.0
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# This is the wrist link not the gripper itself
gripper_frame = 'wrist_roll_link'
# Position and rotation of two "wave end poses"
gripper_poses = [
Pose(Point(0.042, 0.384, 1.826),
Quaternion(0.173, -0.693, -0.242, 0.657)),
Pose(Point(0.047, 0.545, 1.822),
Quaternion(-0.274, -0.701, 0.173, 0.635))
]
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
while not rospy.is_shutdown():
for pose in gripper_poses:
# Finish building the Pose_stamped message
# If the message stamp is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
def __init__(self):
# Give the node a name
rospy.init_node('out_and_back', anonymous=False)
tf_prefix = rospy.get_param('tf_prefix', 'robot1')
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# How fast will we update the robot's movement?
rate = 200
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.2 meters per second
linear_speed = 20
# Set the travel points in meters
goal_x = [
0.0, 0.0, 42.0, 42.0, -12.0, -12.0, -32.0, -32.0, -8.0, -8.0, -8.0,
38.0, 38.0, 0.0, 0.0
]
goal_y = [
0.0, -2.0, -2.0, 52.0, 52.0, 18.0, 18.0, 22.0, 22.0, 18.0, 48.0,
48.0, 2.0, 2.0, 0.0
]
#goal_x = [ 0.0, 0.0, 4.0, 4.0, 5.0, 0.0, 0.0, -5.0,-4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#goal_y = [ 0.0 , 1.0, 1.0, 2.0, 2.0, 2.0,-2.0, -2.0,-1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# Set the rotation speed in radians per second
angular_speed = 0.1
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(5)
distance_tolerance = 0.01
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/' + tf_prefix + '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(
self.odom_frame, '/' + tf_prefix + '/base_footprint',
rospy.Time(), rospy.Duration(1))
self.base_frame = '/' + tf_prefix + '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(
self.odom_frame, '/' + tf_prefix + '/base_link',
rospy.Time(), rospy.Duration(1))
self.base_frame = '/' + tf_prefix + '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
for i in range(len(goal_x)):
print("goal: x=%f, y=%f" % (goal_x[i], goal_y[i]))
move_cmd = Twist()
(position, rotation) = self.get_odom()
angle_to_goal = 0
while not_done(position.x, position.y, goal_x[i], goal_y[i]):
inc_x = goal_x[i] - position.x
inc_y = goal_y[i] - position.y
last_goal = angle_to_goal
angle_to_goal = atan2(inc_y, inc_x)
alpha = angle_to_goal - last_goal
if abs(alpha) > 1.5 * pi:
if alpha > 0:
angle_to_goal -= 2 * pi
else:
angle_to_goal += 2 * pi
# print("goal:")
# print(angle_to_goal)
# print(rotation)
if abs(angle_to_goal -
rotation) > abs(angular_tolerance) or abs(
angle_to_goal -
rotation) > pi - abs(angular_tolerance):
move_cmd.linear.x = 0
if angle_to_goal > rotation:
move_cmd.angular.z = 0.4
else:
move_cmd.angular.z = -0.4
else:
move_cmd.linear.x = 0.1
move_cmd.angular.z = 0.0
self.cmd_vel.publish(move_cmd)
r.sleep()
last_rotation = rotation
(position, rotation) = self.get_odom()
delta = rotation - last_rotation
if abs(delta) > pi:
if delta > 0:
rotation -= 2 * pi
else:
rotation += 2 * pi
# rotation = normalize_angle(rotation)
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(2)
print("All done!")
os.system(
'cd $(rospack find omtb_map_extension_plugin)/scrips && sh save_map_2000.sh'
)
os.system(
'cd $(rospack find omtb_map_extension_plugin)/scrips && sh kill_bag.sh'
)
# Stop the robot for good
self.cmd_vel.publish(Twist())
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
server.insert(int_marker, processFeedback)
menu_handler.apply( server, int_marker.name )
if __name__=="__main__":
rospy.init_node("interactive_static_transform_publisher")
br = tf.TransformBroadcaster()
tf_position = Point(0.0, 0.0, 0.0)
tf_orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
marker_position = Point(tf_position.x + marker_x_offset, tf_position.y, tf_position.z)
if rospy.has_param('~child_frame'):
tf_child_frame = rospy.get_param('~child_frame')
if rospy.has_param('~parent_frame'):
tf_parent_frame = rospy.get_param('~parent_frame')
if rospy.has_param('~tf_publish_period_in_sec'):
tf_update_period = rospy.get_param('~tf_publish_period_in_sec')
server = InteractiveMarkerServer(rospy.get_name()+"/transform_control")
def __init__(self):
#init node
rospy.init_node('exploring_slam',anonymous=True)
rospy.on_shutdown(self.shutdown)
# stop time in target point(s)
self.rest_time = rospy.get_param("~rest_time", 2)
# whether simulink
self.fake_test = rospy.get_param("~fake_test", True)
# goal state
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# set goal position
# click 2D Nav Goal in rviz，the click map in anywhere
locations = dict()
#https://www.jianshu.com/p/17c016778879
locations['1'] = Pose(Point(4.600, -3.2700, 0.000), Quaternion(0.000, 0.000, -0.447, 0.894))
#locations['2'] = Pose(Point(4.231, -6.050, 0.000), Quaternion(0.000, 0.000, -0.847, 0.532))
#locations['3'] = Pose(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
#locations['4'] = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))
#locations['5'] = Pose(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940))
#locations['6'] = Pose(Point(2.924, 0.018, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
#publish the information of control
self.cmd_vel_pub = rospy.Publisher('cmd_vel',Twist,queue_size=5)
#subcribe move_base server msg
self.move_base = actionlib.SimpleActionClient("move_base",MoveBaseAction)
#waiting
self.move_base.wait_for_server(rospy.Duration(60))
#save the init position in rviz
initial_pose = PoseWithCovarianceStamped()
#save succeed runtime and distance value
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
#make sure have init position
while initial_pose.header.stamp == "":
rospy.sleep(1)
#main loop
while not rospy.is_shutdown():
location = '1'
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
#set next target point
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# move
self.move_base.send_goal(self.goal)
# time limit (s)
finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))
# check whether the target point has been reached
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
#run time
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
rospy.sleep(self.rest_time)
# marker orientaiton
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
# marker position
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
# marker line points
marker.points = []
# first point
first_line_point = Point()
first_line_point.x = 4.0
first_line_point.y = -0.527
first_line_point.z = 0.0
marker.points.append(first_line_point)
# second point
second_line_point = Point()
second_line_point.x = 4.0
second_line_point.y = 0.527
second_line_point.z = 0.0
marker.points.append(second_line_point)
# Publish the Marker
pub_line_min_dist.publish(marker)
rospy.sleep(0.5)
