def _odom_sub(self, msg): self.position = msg.pose.pose.position quaternion = msg.pose.pose.orientation orientatioN = efq( [quaternion.x, quaternion.y, quaternion.z, quaternion.w]) self.rotation = orientatioN[2]
def callback_odom(self, msg): self.xr = msg.pose.pose.position.x self.yr = msg.pose.pose.position.y quat = msg.pose.pose.orientation self.phi = efq([quat.x, quat.y, quat.z, quat.w])[2] self.change_odom = True return
def get_current_goal(self): self.current_goal = self.goals[self.goal_id] self.x_goal, self.y_goal = self.current_goal[0], self.current_goal[1] self.qz_goal, self.qw_goal = self.current_goal[2], self.current_goal[3] self.theta_goal = efq([0, 0, self.qz_goal, self.qw_goal]) self.theta_goal = self.theta_goal[2] return
def __odom_sub(self, msg): self.position = msg.pose.pose.position quaternion = msg.pose.pose.orientation orientatioN = efq( [quaternion.x, quaternion.y, quaternion.z, quaternion.w]) self.orientation = Orientation(orientatioN[0], orientatioN[1], orientatioN[2])
def simulate_vehicle(self, current_pose, cmd_vel_list, period=0.05): """ Takes the current position, a list of user inputs, and a step size and simulates the vehicle moving forward in time. """ now = rospy.Time.now().to_sec() time = current_pose.header.stamp.to_sec() current_cmd_vel = cmd_vel_list.pop(0) sim_path = [] state = [current_pose.transform.translation.x, current_pose.transform.translation.y, efq([current_pose.transform.rotation.x, current_pose.transform.rotation.y, current_pose.transform.rotation.z, current_pose.transform.rotation.w])] dk = DifferentialKinematics(1.0, 1.0, state[0], state[1], state[2][2]) # print('Entering sim...') while time < now and not rospy.is_shutdown(): # print(time, now, now - time) # Increase time time += period # Check for new input if cmd_vel_list and current_cmd_vel[0] < time: while cmd_vel_list and cmd_vel_list[0][0] <= time: current_cmd_vel = cmd_vel_list.pop(0) # Use the input, the state, and the period to step the model dk.linear_velocity = current_cmd_vel[1].linear.x dk.angular_velocity = current_cmd_vel[1].angular.z x, y, w = dk.step_simulation(period) sim_path.append([time, x, y, w]) # print('Exiting sim...') return sim_path
def callbackOdom(self, msg): self.x_bot = msg.pose.pose.position.x self.y_bot = msg.pose.pose.position.y quat = msg.pose.pose.orientation self.theta_bot = efq([quat.x, quat.y, quat.z, quat.w])[2] self.change_odom = True return
def get_imu_orientation(self): if self.imu_orientation is None: return 0, 0, 0 else: quat = self.imu_orientation.orientation roll, pitch, yaw = efq((quat.x, quat.y, quat.z, quat.w)) return roll, pitch, yaw
def get_robot_orientation(self): quat = self.current_pose.pose.pose.orientation roll, pitch, yaw = efq((quat.x, quat.y, quat.z, quat.w)) x1, y1 = [np.cos(yaw), np.sin(yaw)] # Give results in quaterion, euler and vector form return quat, (roll, pitch, yaw), [x1, y1]
def callback_pose_user(self, msg): self.pose_user = msg theta_aux = efq( [0, 0, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) #self.thetaUser = np.arccos(theta_aux)*2 self.thetaUser = theta_aux[2] self.change1 = True return
def callback_odom(self, msg): self.pose_walker = msg theta_aux = efq( [0, 0, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) #self.thetaWalker = np.arccos(theta_aux)*2 self.thetaWalker = theta_aux[2] self.change2 = True return
def callback_odom(self, msg): self.x_bot = msg.pose.pose.position.x self.y_bot = msg.pose.pose.position.y self.qz_bot = msg.pose.pose.orientation.z self.qw_bot = msg.pose.pose.orientation.w self.theta_bot = efq([0, 0, self.qz_bot, self.qw_bot]) self.theta_bot = self.theta_bot[2] self.change = True return
def __odom_sub(self, msg): # Calbacck function for odomentry self.position = msg.pose.pose.position quaternion = msg.pose.pose.orientation orientatioN = efq( [quaternion.x, quaternion.y, quaternion.z, quaternion.w]) self.orientation = Orientation(orientatioN[0], orientatioN[1], orientatioN[2])
def callback_tf(self, msg): if msg.transforms[0].header.frame_id == "map" and msg.transforms[ 0].child_frame_id == "odom": qz = msg.transforms[0].transform.rotation.z qw = msg.transforms[0].transform.rotation.w self.rotMapOdom = efq([0, 0, qz, qw])[2] self.tx = msg.transforms[0].transform.translation.x self.ty = msg.transforms[0].transform.translation.y self.change5 = True return
def callback_path(self, msg): self.path = msg try: n = len(self.path.poses) self.nextPose = self.path.poses[n - 1].pose except: self.nextPose = self.path.poses[0].pose theta_aux = efq( [0, 0, self.nextPose.orientation.z, self.nextPose.orientation.w]) self.thetaNav = theta_aux[2] self.change3 = True return
def imu_cb(self, msg): try: (trans, rot) = self.listener.lookupTransform(msg.header.frame_id, '/base_footprint', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException): rospy.logerr('Exception in TF lookup') q = (msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w) (r, p, y) = efq(q) # Raw Reading from the IMU: # 0 is north, and rotations in the CCW direction are positive. # I guess that means N-W-U rospy.loginfo("Raw: " + str(wrapTo360(180.0 * y / math.pi))) # N-W-U to E-N-U is simply a rotation of -90 about the Z axis. tform = quaternion_about_axis(math.pi/2, (0,0,1)) enu = quaternion_multiply(q,tform) (r,p,y) = efq(enu) rospy.loginfo("ENU: " + str(radAndWrap(y))) y = (-y) + math.pi/2.0 rospy.loginfo("NED: " + str(radAndWrap(y)))
def __call__(self, err): """ Assume this is getting a tf2_ros transform """ t = err.header.stamp.to_sec() dxyz = err.transform.translation o = err.transform.rotation _, _, T = efq([o.x, o.y, o.z, o.w]) self.dx(dxyz.x, t) self.dy(dxyz.y, t) self.dz(dxyz.z, t) self.dT(T, t) return self.calc_control(self.dx, self.dy, self.dz, self.dT)
def __call__(self,err): """ Assume this is getting a tf2_ros transform """ t = err.header.stamp.to_sec() dxyz = err.transform.translation o = err.transform.rotation _,_,T = efq([o.x,o.y,o.z,o.w]) self.dx(dxyz.x,t) self.dy(dxyz.y,t) self.dz(dxyz.z,t) self.dT(T,t) return self.calc_control(self.dx,self.dy,self.dz,self.dT)
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 pose_callback(self,pose): if self.goal1 is None and self.goal2 is None: return # Convert pose to x,y,theta (from quaternion orientation) quaternion = N.asarray((pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w)) pose = N.asarray([pose.pose.pose.position.x, pose.pose.pose.position.y,efq(quaternion)[2]]) self.goals.update_pose(pose) if self.goals.next_goal_flag: self.string_msg.data = str(self.goals.last_distance) self.acc_pub.publish(self.string_msg) self.string_msg.data = str(self.goals.current_goal) self.goal_pub.publish(self.string_msg)
def imu_cb(self, data): if self.decimate_ahrs not in [0, 1]: if self.ahrs_count % self.decimate_ahrs == 0: self.ahrs_count = 1 else: self.ahrs_count += 1 return if not self.imu_used: if self.heading_initilized: return if not self.heading_initilized: self.heading_initilized = True (r, p, yaw) = efq([data.orientation.x, data.orientation.y, \ data.orientation.z, data.orientation.w]) # This is supposed to correct for the coordinate differences self.ekf.measurementUpdateAHRS(yaw + np.pi / 2) self.filter_time = data.header.stamp return
def listen(self, msg): # quaternion = [0, 0, 0, 0] # quaternion[0] = msg.pose.orientation.x # quaternion[1] = msg.pose.orientation.y # quaternion[2] = msg.pose.orientation.z # quaternion[3] = msg.pose.orientation.w euler = efq((msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)) roll = euler[0] pitch = euler[1] yaw = euler[2] x = msg.pose.position.x y = msg.pose.position.y z = msg.pose.position.z #self.setpoint_pos_pub.publish(0) #self.state_pos_pub.publish(z) self.setpoint_angle_pub.publish(0) self.state_angle_pub.publish(x - (0.531 * z + 0.02043))
def transform(PoseStamped): pose = PoseStamped.pose pos = pose.position orient = pose.orientation # Simple rotations from ENU to NED... ish tfpos = array([pos.y,pos.x,-pos.z]) tforient = efq([orient.x,orient.y,orient.z,orient.w]) # Transform from quat to euler happens in ENU frame. # Rotate to match NED tforient = array([-tforient[2],tforient[0],tforient[1]]) # Desired position. This should be read in from a file, or better # yet. should come from a subscription to a topic somehow. Still learning... des_pos = array([0,0,-1.5]) pos_err = des_pos - tfpos # Keep yaw turned towards +x des_yaw = 0 yaw_err = des_yaw - tforient[0] vel_set(pos_err,yaw_err)
def pose_callback(self, pose): if self.goal1 is None and self.goal2 is None: return # Convert pose to x,y,theta (from quaternion orientation) quaternion = N.asarray( (pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w)) pose = N.asarray([ pose.pose.pose.position.x, pose.pose.pose.position.y, efq(quaternion)[2] ]) self.goals.update_pose(pose) if self.goals.next_goal_flag: self.string_msg.data = str(self.goals.last_distance) self.acc_pub.publish(self.string_msg) self.string_msg.data = str(self.goals.current_goal) self.goal_pub.publish(self.string_msg)
def detect_target(self,data): # find yellow blob hsv = cv2.cvtColor(data,cv2.COLOR_BGR2HSV) yel_lo = (20,100,100) yel_hi = (30,255,255) mask = cv2.inRange(hsv,yel_lo,yel_hi) _,cnts,heir = cv2.findContours(mask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) if not len(cnts): # no yellow(target) in image return data,None,None,None target = sorted(cnts,key=lambda c:cv2.contourArea(c))[-1] leftmost = tuple(target[target[:,:,0].argmin()][0]) rightmost = tuple(target[target[:,:,0].argmax()][0]) topmost = tuple(target[target[:,:,1].argmin()][0]) bottommost = tuple(target[target[:,:,1].argmax()][0]) if (leftmost[0] == 0 or topmost[1] == 0 or rightmost[0] == self.cam_info.width or bottommost[1] == self.cam_info.height): #target is at least partially occluded by edge of frame # rospy.loginfo('{0}: frame occlusion. not trusting estimate'.format('vision_estimator')) return data,None,None,None try: ellipse_rect = cv2.fitEllipse(target) except: #FIXME: need to capture only OpenCV Error #error: OpenCV Error: Incorrect size of input array (There should be at least 5 points to fit the ellipse) in fitEllipse return data,None,None,None #TODO: handle case where we are too close to see the platform and also when we just can't see a platform at all if not abs(1 - (ellipse_rect[1][0] - ellipse_rect[1][1])) > 0.2: dist = self.dz - 0.1 #assume we're really close and we've lost the platform a little else: area = cv2.contourArea(target) diam = np.sqrt(4*area/np.pi) dist = 0.255*2*self.fy/diam # actual radius of landing pad = 255mm dz = dist # not a very good guess, especially if we're tilted M = cv2.moments(target) if not M["m00"]: return mask,None, None,None Pi = np.matrix((M["m10"]/M["m00"],M["m01"]/M["m00"],1)).T # point in image coords Pc = self.K.I*Pi # point in world units (still on sensor) Pc = Pc/Pc[2] * dist # point projected onto ground plane Pc[1] *= -1 # account for image coordinates astarting in top left o = self.orientation q = [o.x,o.y,o.z,o.w] r,p,y = efq(q,'sxyz') Rcam_body = np.matrix(eul_mat(3.14,0,0))[:3,:3] Rbody_inert = np.matrix(eul_mat(r,p,-y))[:3,:3] Pr = Rbody_inert*Rcam_body.T*Pc dx = Pr[0] dy = -Pr[1] # invert y to make sane coordinate system (right:x+,up:y+) #print('dx',float(Pr[0]),' dy',float(Pr[1]),'rpy',r,p,-y) # build message for estimator pose = Pose() # rotate +x to +y and +y to -x to reflect the fact that +x is out the nose of the vehicle pose.position.x = -dy pose.position.y = dx pose.position.z = dz pose.orientation = o pose_w_cov = PoseWithCovariance( pose=pose, covariance=self.cov) odom = Odometry(pose=pose_w_cov) odom.header.stamp = rospy.Time.now() odom.header.frame_id = 'pad' odom.child_frame_id = 'camera::camera_link' self.odom_pub.publish(odom) ci = ((data.shape[0]/2),(data.shape[1]/2)) heading = (Pi[0]-ci[0],ci[1]-Pi[1]) return mask,heading,tuple(Pi[:2]),ci
def wp_pub_sub(): global curr_x,curr_y,curr_orient,next_wp,ready_pub,spin,mode,size,rtl,all_waypoints,first_pub next_wp.pose.position.z = 0.4 all_waypoints = Waypoints() rospy.init_node('UAV_setpoint',anonymous=True) rospy.Subscriber('next_wps',Waypoints,setpoints) rospy.Subscriber('/mavros/state',State,get_curr_mode) rospy.Subscriber('slam_out_pose',PoseStamped,set_curr) rospy.Subscriber('/mavros/local_position/pose',PoseStamped,set_curr_z) rospy.Subscriber('return_to_launch',Bool,get_rtl) rate = rospy.Rate(15) wp_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) ready_pub = rospy.Publisher('ready_for_wps', Bool, queue_size=10) rtl_pub = rospy.Publisher('return_to_launch', Bool, queue_size=10) rtl_path_pub = rospy.Publisher('next_wps', Waypoints, queue_size=10) first_pub = rospy.Publisher('first_wps', Bool, queue_size=10) spin_flag = 0 spin = False spins = 0 offboard_counter = 0 rtl_flag = 0 size = 128 while not rospy.is_shutdown(): if 'mode' in globals() and mode in "OFFBOARD": offboard_counter += 1 #rospy.loginfo(str(offboard_counter)) if 0 > offboard_counter <= 5: rospy.loginfo("Just entered OFFBOARD mode") pos = PoseStamped() pos.header.stamp = rospy.Time.now() pos.pose.position.x = next_wp.pose.position.x #these lines could also be changed instead of above pos.pose.position.y = next_wp.pose.position.y pos.pose.position.z = next_wp.pose.position.z #this line can be changed to reflect desired z setpoints #if 'curr_z' in globals(): # rospy.loginfo('spins:%s,curr_z:%s,offboard_counter:%s',str(spins),str(curr_z),str(offboard_counter)) if spins == 0 and 'curr_z' in globals() and curr_z > 0.15 and offboard_counter >= 50: rospy.loginfo("executing first spin maneuver") spin = True first_pub.publish(True) if spins%5: spin = False if spin: # Yaw is in /slam_out_pose frame. if spin_flag == 1: rospy.loginfo("Yaw Left") yaw = pi/8 elif spin_flag == 0: rospy.loginfo("Yaw Right") yaw = -pi/8 else: rospy.loginfo("Yaw Back") yaw = 0 quat = qfe(0,0,yaw+pi/2)#euler angles -- RPY roll pitch yaw --> Q xyzw #add pi/2 to send to setpoint_position/local pos.pose.orientation.x = quat[0] pos.pose.orientation.y = quat[1] pos.pose.orientation.z = quat[2] pos.pose.orientation.w = quat[3] wp_pub.publish(pos) while not 'curr_orient' in globals(): time.sleep(0.01) curr_yaw = efq([curr_orient.x,curr_orient.y,curr_orient.z,curr_orient.w])[-1] #rospy.loginfo('curr_yaw: %s, des_yaw: %s',str(curr_yaw),str(yaw)) epsilon = 0.1 if abs(curr_yaw - yaw) < epsilon: spin_flag += 1 if yaw == 0: spin = False spin_flag = 0 ready_pub.publish(True) #ready for more waypoints. send nav_map to sentel spins += 1 rate.sleep() continue # this tells us to skip everything else in the loop and start from the top, (skip next 7 lines) if 'rtl' in globals() and rtl and not rtl_flag: rtl_flag = 1 rtl_pub.publish(True) rtl_wps = Waypoints() rtl_wps.x = list(reversed(all_waypoints.x)) rtl_wps.y = list(reversed(all_waypoints.y)) rtl_path_pub.publish(rtl_wps) quat = qfe(0,0,pi/2) pos.pose.orientation.x = quat[0] pos.pose.orientation.y = quat[1] pos.pose.orientation.z = quat[2] pos.pose.orientation.w = quat[3] wp_pub.publish(pos) rate.sleep()
def wp_pub_sub(): global curr_x,curr_y,curr_orient,next_wp,ready_pub,spin,state_id next_wp.pose.position.z = 0.5 rospy.init_node('UAV_setpoint',log_level=rospy.DEBUG,anonymous=True) rospy.Subscriber('next_wps',Waypoints,setpoints) rospy.Subscriber('slam_out_pose',PoseStamped,set_curr) rate = rospy.Rate(15) wp_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) ready_pub = rospy.Publisher('ready_for_wps', Bool, queue_size=10) spin_flag = 0 spin = False while not rospy.is_shutdown(): if state_id is 1: pos = PoseStamped() pos.header.stamp = rospy.Time.now() pos.pose.position.x=0 pos.pose.position.y=0 pos.pose.position.z=0 quat = qfe(0,0,pi/2) pos.pose.orientation.w = quat[3] pos.pose.orientation.x = quat[0] pos.pose.orientation.y = quat[1] pos.pose.orientation.z = quat[2] pub.publish(pos) elif state_id is 2: count = +1 if count < 150: pos = PoseStamped() pos.header.stamp = rospy.Time.now() pos.pose.position.x=0 pos.pose.position.y=0 pos.pose.position.z=0 quat = qfe(0,0,pi/2) pos.pose.orientation.w = quat[3] pos.pose.orientation.x = quat[0] pos.pose.orientation.y = quat[1] pos.pose.orientation.z = quat[2] pub.publish(pos) pos = PoseStamped() pos.header.stamp = rospy.Time.now() pos.pose.position.x = -next_wp.pose.position.y #these lines could also be changed instead of above pos.pose.position.y = next_wp.pose.position.x pos.pose.position.z = next_wp.pose.position.z #this line can be changed to reflect desired z setpoints if curr_z > 0.3: spin = True if spin: # Yaw is in /slam_out_pose frame. if spin_flag == 1: rospy.loginfo("Yaw Left") yaw = 3*pi/4 elif spin_flag == 0: rospy.loginfo("Yaw Right") yaw = -3*pi/4 else: rospy.loginfo("Yaw Back") yaw = 0 quat = qfe(0,0,yaw+pi/2)#euler angles -- RPY roll pitch yaw --> Q xyzw #add pi/2 to send to setpoint_position/local pos.pose.orientation.x = quat[0] pos.pose.orientation.y = quat[1] pos.pose.orientation.z = quat[2] pos.pose.orientation.w = quat[3] wp_pub.publish(pos) while not 'curr_orient' in globals(): time.sleep(0.01) curr_yaw = efq([curr_orient.x,curr_orient.y,curr_orient.z,curr_orient.w])[-1] rospy.loginfo('curr: %s, des: %s',str(curr_yaw),str(yaw)) epsilon = 0.1 if abs(curr_yaw - yaw) < epsilon: spin_flag += 1 if yaw == 0: spin = False spin_flag = 0 ready_pub.publish(True) #ready for more waypoints. send nav_map to sentel rate.sleep() continue # this tells us to skip everything else in the loop and start from the top, (skip next 7 lines) quat = qfe(0,0,pi/2) pos.pose.orientation.x = quat[0] pos.pose.orientation.y = quat[1] pos.pose.orientation.z = quat[2] pos.pose.orientation.w = quat[3] wp_pub.publish(pos) rate.sleep()
import rospy import tf2_ros from geometry_msgs.msg import Twist from tf.transformations import euler_from_quaternion as efq rospy.init_node('tf_dist') tf_dist_pub = rospy.Publisher('irols/tf_dist', Twist, queue_size=1) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) while not rospy.is_shutdown(): rate = rospy.Rate(10) dist = Twist() try: t = tfBuffer.lookup_transform('pad', 'base_link', rospy.Time(0)) dist.linear.x = t.transform.translation.x dist.linear.y = t.transform.translation.y dist.linear.z = t.transform.translation.z o = t.transform.rotation dist.angular.x, dist.angular.y, dist.angular.z = efq( [o.x, o.y, o.z, o.w]) tf_dist_pub.publish(dist) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: print e rate.sleep() continue
def __odom_callback(self, msg): self.position = msg.pose.pose.position orientatioN = efq([msg.pose.pose.orientaion.x, msg.pose.pose.orientaion.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation]) self.orientation = Orientation(orientatioN[0], orientatioN[1], orientatioN[2])
def callbackGoal(self, msg): self.x_goal = msg.pose.position.x self.y_goal = msg.pose.position.y quat = msg.pose.orientation self.theta_goal = efq([quat.x, quat.y, quat.z, quat.w])[2] self.change_goal = True
#!/usr/bin/env python import rospy import tf2_ros from geometry_msgs.msg import Twist from tf.transformations import euler_from_quaternion as efq rospy.init_node('tf_dist') tf_dist_pub = rospy.Publisher('irols/tf_dist',Twist,queue_size=1) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) while not rospy.is_shutdown(): rate = rospy.Rate(10) dist = Twist() try: t = tfBuffer.lookup_transform('pad','base_link',rospy.Time(0)) dist.linear.x = t.transform.translation.x dist.linear.y = t.transform.translation.y dist.linear.z = t.transform.translation.z o = t.transform.rotation dist.angular.x, dist.angular.y, dist.angular.z = efq([o.x,o.y,o.z,o.w]) tf_dist_pub.publish(dist) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: print e rate.sleep() continue