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