def _publish_odom(self, timestamp): ''' See base class comment ''' # One of the kalman filters have not been initialized yet, don't publish if not self._chassis_kf or not self._imu_kf: return if self._should_reset(): rospy.logwarn("Odometry diverged, resetting kalman filters...") self._reset_kf() return odom_msg = Odometry() odom_msg.header.frame_id = "odom" odom_msg.child_frame_id = "base_center" # fitting 2 frame transforms into 1 odom msg odom_msg.header.stamp = timestamp chassis_state = self._chassis_kf.statePost[:, 0] imu_state = self._imu_kf.statePost[:, 0] # Translation (odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y) = chassis_state[0:2] odom_msg.pose.pose.position.z = 0 # Rotation mod = 2 * math.pi mcb_imu_quat = tf_conversions.transformations.quaternion_from_euler( imu_state[0] % mod, imu_state[1] % mod, imu_state[2] % mod) pose_quat = convert_quat_to_frame(mcb_imu_quat, "base_center", "mcb_imu") (odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y, odom_msg.pose.pose.orientation.z, odom_msg.pose.pose.orientation.w) = pose_quat # Set pose covariances chassis_cov = self._chassis_kf.errorCovPost imu_cov = self._imu_kf.errorCovPost pose_cov_matrix = np.zeros((6, 6), dtype=np.float32) pose_cov_matrix[0:2, 0:2] = chassis_cov[0:2, 0:2] pose_cov_matrix[3:6, 3:6] = imu_cov[0:3, 0:3] odom_msg.pose.covariance = tuple(pose_cov_matrix.flatten()) # Translational velocity (odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y) = chassis_state[2:4] odom_msg.twist.twist.linear.z = 0 # Angular velocities twist_euler = convert_angv_to_frame(imu_state[3:6], "base_center", "mcb_imu") (odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y, odom_msg.twist.twist.angular.z) = twist_euler[0:3] # Set velocity covariances twist_cov_matrix = np.zeros((6, 6), dtype=np.float32) twist_cov_matrix[0:2, 0:2] = chassis_cov[2:4, 2:4] twist_cov_matrix[3:6, 3:6] = imu_cov[3:6, 3:6] odom_msg.twist.covariance = tuple(twist_cov_matrix.flatten()) self._odom_pub.publish(odom_msg)
def __init__(self, state_collector, ns, robot_radius=0, robot_width=0.58, robot_height=0.89): self.__robot_radius = robot_radius # robot radius self.__robot_height = robot_height # robot width self.__robot_width = robot_width # robot heigth self.__odom = Odometry( ) # most recently published odometry of the robot self.__path = Path() # most recently published path self.__done = False # is episode done? self.__new_task_started = False # has a new task started? self.__state_collector = state_collector # for getting relevant state values of the robot self.__rl_agent_path = rospkg.RosPack().get_path( 'rl_agent') # absolute path rl_agent-package self.__flatland_topics = [] # list of flatland topics self.__timestep = 0 # actual timestemp of training self.__NS = ns self.MODE = rospy.get_param("%s/rl_agent/train_mode", 1) self.__clock = Clock().clock self.__task_generator = TaskGenerator(self.__NS, self.__state_collector, 0.46) self.__recent_agent_states = [] # Subscriber for getting data #self.__odom_sub = rospy.Subscriber("%s/odom"%self.__NS, Odometry, self.__odom_callback, queue_size=1) self.__global_plan_sub = rospy.Subscriber( "%s/move_base/NavfnROS/plan" % self.__NS, Path, self.__path_callback, queue_size=1) # self.__path_sub = rospy.Subscriber("%s/move_base/GlobalPlanner/plan" % self.__NS, Path, self.__path_callback, queue_size=1) self.__done_sub = rospy.Subscriber("%s/rl_agent/done" % self.__NS, Bool, self.__done_callback, queue_size=1) self.__new_task_sub = rospy.Subscriber('%s/rl_agent/new_task_started' % self.__NS, Bool, self.__new_task_callback, queue_size=1) self.__flatland_topics_sub = rospy.Subscriber( "%s/flatland_server/debug/topics" % self.__NS, DebugTopicList, self.__flatland_topic_callback, queue_size=1) self.__agent_action_sub = rospy.Subscriber( '%s/rl_agent/action' % self.__NS, Twist, self.__trigger_callback) self.__ped_sub = rospy.Subscriber( '%s/pedsim_simulator/simulated_agents' % self.__NS, AgentStates, self.__ped_callback) if self.MODE == 1 or self.MODE == 0: self.clock_sub = rospy.Subscriber('%s/clock' % self.__NS, Clock, self.__clock_callback) # Publisher for generating qualitative image self.__driven_path_pub = rospy.Publisher('%s/rl_eval/driven_path' % self.__NS, Path, queue_size=1) self.__driven_path_pub2 = rospy.Publisher('%s/rl_eval/driven_path2' % self.__NS, Path, queue_size=1) self.__global_path_pub = rospy.Publisher('%s/rl_eval/global_path' % self.__NS, Path, queue_size=1) self.__agent_pub = rospy.Publisher('%s/rl_eval/viz_agents' % self.__NS, MarkerArray, queue_size=1)
def publish_messages(): # pub = rospy.Publisher('ekf_data_fused', Odometry, queue_size=10) # rospy.init_node('mavlink_manager_in', anonymous=True) # rate = rospy.Rate(5) # 10hz # while not rospy.is_shutdown(): # current_time = rospy.Time.now() # ekf_data_fused = connection_in.recv_match(type='ODOMETRY', blocking=True) # connection_in.messages['Odometry'] # # print("ecco sta merda di msg") # # print(connection_in) # ekf_data_fused_ros = Odometry() # ekf_data_fused_ros.header.stamp = current_time # ekf_data_fused_ros.header.frame_id = "odom" # ekf_data_fused_ros.pose.pose.position.x = ekf_data_fused.x # ekf_data_fused_ros.pose.pose.position.y = ekf_data_fused.y # ekf_data_fused_ros.pose.pose.orientation.w = ekf_data_fused.q[0] # ekf_data_fused_ros.pose.pose.orientation.x = ekf_data_fused.q[3] # ekf_data_fused_ros.twist.twist.linear.x = ekf_data_fused.vx # ekf_data_fused_ros.twist.twist.linear.y = ekf_data_fused.vy # pub.publish(ekf_data_fused_ros) # rate.sleep() pub_enc = rospy.Publisher('encoder', Odometry, queue_size=10) pub_mag = rospy.Publisher('magnetometer', Odometry, queue_size=10) pub_imu = rospy.Publisher('imu_k64', Imu, queue_size=10) pub_imu2 = rospy.Publisher('imu_ext', Imu, queue_size=10) global pos_l_old global pos_l_new global pos_r_old global pos_r_new global psi_old global oldT oldT = 0 pos_l_old = 0 pos_l_new = 0 pos_r_old = 0 pos_r_new = 0 psi_old = 0 x_0 = 0 y_0 = 0 r = 0.02 B = 0.185 rospy.init_node('mavlink_manager_in', anonymous=True) rate = rospy.Rate(20) # 10hz while not rospy.is_shutdown(): current_time = rospy.Time.now() # imu = connection_in.recv_match(type='SCALED_IMU', blocking=True) imu_ros = Imu() imu_ros.header.stamp = current_time imu_ros.header.frame_id = "odom" # odom before imu_ros.linear_acceleration.x = 0 imu_ros.linear_acceleration.y = 0 imu_ros.linear_acceleration.z = 0 imu_ros.linear_acceleration_covariance[0] = 0 imu_ros.linear_acceleration_covariance[4] = 0 imu_ros.linear_acceleration_covariance[8] = 0 # imu_ros.orientation.z = imu.zmag/10000 # imu_ros.orientation.w = sqrt(1-(imu.zmag/10000)**2) # imu_ros.orientation.x = 0 # imu_ros.orientation.y = 0 imu_ros.orientation_covariance[8] = 0 pub_imu.publish(imu_ros) # rospy.loginfo("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA") # Magnetometer is treated as a separate Odom topic magneto_ros = Odometry() magneto_ros.header.stamp = current_time magneto_ros.header.frame_id = "odom" magneto_ros.pose.pose.orientation.z = 0 magneto_ros.pose.pose.orientation.w = 0 magneto_ros.pose.pose.orientation.x = 0 magneto_ros.pose.pose.orientation.y = 0 pub_mag.publish(magneto_ros) # imu_ex = connection_in.recv_match(type='SCALED_IMU2', blocking=True) imu_ros2 = Imu() imu_ros2.header.stamp = current_time imu_ros2.header.frame_id = "odom" # odom before imu_ros2.linear_acceleration.x = 0 imu_ros2.linear_acceleration.y = 0 imu_ros2.linear_acceleration.z = 0 imu_ros2.linear_acceleration_covariance[0] = 0 imu_ros2.linear_acceleration_covariance[4] = 0 imu_ros2.linear_acceleration_covariance[8] = 0 imu_ros2.angular_velocity.z = 0 imu_ros2.angular_velocity.x = 0 imu_ros2.angular_velocity.y = 0 imu_ros2.orientation.z = 0 imu_ros2.orientation.w = 0 imu_ros2.orientation.x = 0 imu_ros2.orientation.y = 0 imu_ros2.angular_velocity_covariance[0] = 2.5 / 10000 imu_ros2.angular_velocity_covariance[4] = 2.7 / 10000 imu_ros2.angular_velocity_covariance[8] = 2.6 / 10000 pub_imu2.publish(imu_ros2) # encoders = connection_in.recv_match(type='WHEEL_DISTANCE', blocking=True) # connection_in.messages['Odometry'] # # print(connection_in) # if pos_l_old== 0: # pos_l_old = encoders.distance[0] # pos_r_old = encoders.distance[1] # psi_old = 0 encoders_ros = Odometry() encoders_ros.header.stamp = current_time encoders_ros.header.frame_id = "odom" rot = R.from_quat([0, 0, 0, 1]) [psi, _, _] = rot.as_euler("zyx", degrees=False) # Integration of psi based on encoder positions pos_l_new = 1 pos_r_new = 1 # currentT = (float)encoders_ros.header.stamp # deltaT = currentT - oldT # oldT = currentT deltaPos_l = pos_l_new - pos_l_old deltaPos_r = pos_r_new - pos_r_old psi_new = psi_old + (1 / B) * (deltaPos_r - deltaPos_l) # Rad VR = deltaPos_r VL = deltaPos_l pos_l_old = pos_l_new pos_r_old = pos_r_new psi_old = psi_new # if (x_0 == 0) and (y_0 == 0): # x_0 = (encoders.distance[0]+encoders.distance[1])/2*cos(psi_new) # y_0 = (encoders.distance[0]+encoders.distance[1])/2*sin(psi_new) # encoders_ros.pose.pose.position.x = (encoders.distance[0]+encoders.distance[1])/2*cos(psi_new) #-x_0 # encoders_ros.pose.pose.position.y = (encoders.distance[0]+encoders.distance[1])/2*sin(psi_new) #-y_0 encoders_ros.pose.pose.position.x = x_0 + (VR + VL) / 2 * cos(psi_new) encoders_ros.pose.pose.position.y = y_0 + (VR + VL) / 2 * sin(psi_new) x_0 = encoders_ros.pose.pose.position.x y_0 = encoders_ros.pose.pose.position.y print('Psi value from frdm', psi_new * 180 / pi, encoders_ros.pose.pose.position.x, encoders_ros.pose.pose.position.y, (pos_l_new + pos_r_new), x_0, y_0) # encoders_ros.pose.pose.position.y = 0 pub_enc.publish(encoders_ros) rate.sleep()
def _get_obs(self): """ Read robot state :return: """ rospy.logdebug("Start Get Observation ==>") # Pose of the MAV pose = self.get_gt_odom() stamp = pose.header.stamp # Velocity of the MAV #vel = self.get_velocity(stamp) #print(vel.twist.twist) velocity = self.get_tello_velocity() vel = Odometry() vel.twist.twist.linear.x = velocity[1] vel.twist.twist.linear.y = velocity[0] vel.twist.twist.linear.z = -velocity[2] print(vel.twist.twist) # Position of the Target target = self.get_gt_target(stamp) if target is None: target = Odometry() target.pose.pose.position.x = 0 target.pose.pose.position.y = 0 target.pose.pose.position.z = 0 target.twist.twist.linear.x = 0 target.twist.twist.linear.y = 0 target.twist.twist.linear.z = 0 ################################################################################################### ''' Inputs for Policy Network ''' ################################################################################################### '''Actor Root in Mav base link frame: Only Translation. Also used for calculating MAV bearing''' delta_target1 = PoseWithCovarianceStamped() delta_target_w = np.array([target.pose.pose.position.y-pose.position.y,\ target.pose.pose.position.x-pose.position.x,\ -(target.pose.pose.position.z-pose.position.z)]) TARGET_orientation = np.array([ target.pose.pose.orientation.x, target.pose.pose.orientation.y, target.pose.pose.orientation.z, target.pose.pose.orientation.w ]) MAV_orientation = np.array([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]) (r, p, y) = tf.transformations.euler_from_quaternion(TARGET_orientation) TARGET_yaw = y (r, p, y) = tf.transformations.euler_from_quaternion(MAV_orientation) MAV_yaw = y homogeneous_matrix = tf.transformations.euler_matrix(0, 0, y, axes='sxyz') inv_homogeneous_matrix = np.linalg.inv(homogeneous_matrix[:3, :3]) delta_target_local = np.dot(inv_homogeneous_matrix, delta_target_w) #Relative Translation delta_target1.pose.pose.position.x = delta_target_local[0] delta_target1.pose.pose.position.y = delta_target_local[1] delta_target1.pose.pose.position.z = delta_target_local[2] #Bearing theta1 = np.arctan2(delta_target_local[1], delta_target_local[0]) '''Actor Root Orientation in Mav base link frame: Only Rotation ''' delta_orient = TARGET_yaw - MAV_yaw ''' Take smaller difference between angles''' if delta_orient > np.pi: delta_orient += -2 * np.pi elif delta_orient < -np.pi: delta_orient += 2 * np.pi else: delta_orient += 0 '''Calculate relative velocity vector in the MAV base link frame''' relative_vel = np.dot(inv_homogeneous_matrix, np.array([vel.twist.twist.linear.y - target.twist.twist.linear.x, vel.twist.twist.linear.x - target.twist.twist.linear.y,\ -vel.twist.twist.linear.z - target.twist.twist.linear.z])) #Dummy zero inputs to value network during testing. value_input = [0] * 55 scale = 4 # Policy Inputs + Value Inputs observations = [scale*delta_target1.pose.pose.position.x, scale*delta_target1.pose.pose.position.y, scale*delta_target1.pose.pose.position.z,\ relative_vel[0],relative_vel[1],relative_vel[2],np.cos(theta1), np.sin(theta1),np.cos(delta_orient), np.sin(delta_orient)]+value_input #rospy.loginfo("Observations==>"+str(observations)) #rospy.loginfo("END Get Observation ==>") return observations
cont = 0 #Node and msg initialization rospy.init_node('path_odom_plotter') #Rosparams that are set in the launch #max size of array pose msg from the path if not rospy.has_param("~max_list_append"): rospy.logwarn('The parameter max_list_append dont exists') max_append = rospy.set_param("~max_list_append", 1000) max_append = 1000 if not (max_append > 0): rospy.logwarn('The parameter max_list_append not is correct') sys.exit() pub = rospy.Publisher('/odompath', Path, queue_size=1) path = Path() #creamos el mensaje path de tipo path msg = Odometry() #Subscription to the topic msg = rospy.Subscriber('/odom', Odometry, callback) rate = rospy.Rate(30) # 30hz try: while not rospy.is_shutdown(): #rospy.spin() rate.sleep() except rospy.ROSInterruptException: pass
def main(self): print("starting node") rospy.init_node('rs_to_odom') listener = tf.TransformListener() odom_sub = rospy.Subscriber("/t265/odom/sample", Odometry, self.odom_callback) initial_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.initial_callback) odom_pub = rospy.Publisher("/base_pose_ground_truth", Odometry , queue_size=10) move_base_odom_pub = rospy.Publisher("/odom", Odometry , queue_size=10) odom_msg = Odometry() odom_msg.header.frame_id = "odom" odom_msg.child_frame_id = "base_link" odom_msg.pose.covariance = [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001] odom_msg.twist.covariance = [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001] while not rospy.is_shutdown(): try: rospy.loginfo("odom info recieved %s"%(str([[lx,ly,lz],[ax,ay,az]]))) rospy.loginfo("beginning odometry transformation") except NameError: continue else: break #lx, ly, lz, ax, ay, az = (0,0,0,0,0,0) #time.sleep(0.1) # allows listener buffer to load + callback variables rate = rospy.Rate(50) try: while not rospy.is_shutdown(): try: (trans,rot) = listener.lookupTransform('/t265_odom_frame', '/fake_base_link', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue # Negate to make it start "backwards" odom_msg.pose.pose.position.x = trans[0] - 0.42545 + self.new_x_pose odom_msg.pose.pose.position.y = trans[1] + self.new_y_pose odom_msg.pose.pose.position.z = 0 odom_msg.pose.pose.orientation.x = 0 odom_msg.pose.pose.orientation.y = 0 # Inorder to flip robot 180 degrees you must switch the z and w values: negate values to switch turn direction odom_msg.pose.pose.orientation.z = rot[2] odom_msg.pose.pose.orientation.w = rot[3] #print("z: ",rot[2]," w: ", rot[3]) odom_msg.twist.twist.linear.x = -lx #odom_msg.twist.twist.linear.y = ly #odom_msg.twist.twist.linear.z = lz odom_msg.twist.twist.angular.x = ax odom_msg.twist.twist.angular.y = ay odom_msg.twist.twist.angular.z = az now = rospy.get_rostime() odom_msg.header.stamp.secs = now.secs odom_msg.header.stamp.nsecs = now.nsecs odom_pub.publish(odom_msg) # For move_base move_base_odom_pub.publish(odom_msg) rate.sleep() except rospy.ROSInterruptException: pass
def cb_odometry(self, msg): # Nan check if (math.isnan(msg.pose.pose.position.x) or math.isnan(msg.pose.pose.position.y) or math.isnan(msg.pose.pose.position.z) or math.isnan(msg.pose.pose.orientation.x) or math.isnan(msg.pose.pose.orientation.y) or math.isnan(msg.pose.pose.orientation.z) or math.isnan(msg.twist.twist.linear.x) or math.isnan(msg.twist.twist.linear.y) or math.isnan(msg.twist.twist.linear.z) or math.isnan(msg.twist.twist.angular.x) or math.isnan(msg.twist.twist.angular.y) or math.isnan(msg.twist.twist.angular.z)): rospy.logwarn('Recieved an odom message with nan values') return pos_t265_odombased = np.array([ msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z ]) quat_t265_odombased = Quaternion( msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z).normalised pos_base_odombased = pos_t265_odombased + \ quat_t265_odombased.rotate(self._pos_base_t265based) quat_base_odombased = quat_t265_odombased * self._quat_base_t265based pos_dot_t265_t265based = np.array([ msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.linear.z ]) omega_t265_t265based = np.array([ msg.twist.twist.angular.x, msg.twist.twist.angular.y, msg.twist.twist.angular.z ]) pos_dot_base_basebased = self._quat_t265_basebased.rotate( pos_dot_t265_t265based + np.cross(omega_t265_t265based, self._pos_base_t265based)) omega_base_basebased = self._quat_t265_basebased.rotate( omega_t265_t265based) pub_msg = Odometry() pub_msg.header.stamp = rospy.Time.now() pub_msg.header.frame_id = self._frame_id_odom pub_msg.child_frame_id = self._frame_id_base_link if self._2d_mode: quat_base_odombased_2d = Quaternion( quat_base_odombased.w, 0, 0, quat_base_odombased.z).normalised pub_msg.pose.pose.position.x = pos_base_odombased[0] pub_msg.pose.pose.position.y = pos_base_odombased[1] pub_msg.pose.pose.position.z = 0 pub_msg.pose.pose.orientation.x = quat_base_odombased_2d.x pub_msg.pose.pose.orientation.y = quat_base_odombased_2d.y pub_msg.pose.pose.orientation.z = quat_base_odombased_2d.z pub_msg.pose.pose.orientation.w = quat_base_odombased_2d.w pub_msg.twist.twist.linear.x = pos_dot_base_basebased[0] pub_msg.twist.twist.linear.y = pos_dot_base_basebased[1] pub_msg.twist.twist.linear.z = 0 pub_msg.twist.twist.angular.x = 0 pub_msg.twist.twist.angular.y = 0 pub_msg.twist.twist.angular.z = omega_base_basebased[2] else: pub_msg.pose.pose.position.x = pos_base_odombased[0] pub_msg.pose.pose.position.y = pos_base_odombased[1] pub_msg.pose.pose.position.z = pos_base_odombased[2] pub_msg.pose.pose.orientation.x = quat_base_odombased.x pub_msg.pose.pose.orientation.y = quat_base_odombased.y pub_msg.pose.pose.orientation.z = quat_base_odombased.z pub_msg.pose.pose.orientation.w = quat_base_odombased.w pub_msg.twist.twist.linear.x = pos_dot_base_basebased[0] pub_msg.twist.twist.linear.y = pos_dot_base_basebased[1] pub_msg.twist.twist.linear.z = pos_dot_base_basebased[2] pub_msg.twist.twist.angular.x = omega_base_basebased[0] pub_msg.twist.twist.angular.y = omega_base_basebased[1] pub_msg.twist.twist.angular.z = omega_base_basebased[2] # TODO: tranform covariance pub_msg.pose.covariance = msg.pose.covariance pub_msg.twist.covariance = msg.twist.covariance self._pub_odom.publish(pub_msg) if self._publish_tf: t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = pub_msg.header.frame_id t.child_frame_id = pub_msg.child_frame_id t.transform.translation.x = pub_msg.pose.pose.position.x t.transform.translation.y = pub_msg.pose.pose.position.y t.transform.translation.z = pub_msg.pose.pose.position.z t.transform.rotation.x = pub_msg.pose.pose.orientation.x t.transform.rotation.y = pub_msg.pose.pose.orientation.y t.transform.rotation.z = pub_msg.pose.pose.orientation.z t.transform.rotation.w = pub_msg.pose.pose.orientation.w self._tf_br.sendTransform(t)
def trajectory_in_camera_fov(self, data): """generates trajectory in camera workspace""" #self.end_point = self.end_point + np.array([0, -self.target_velocity*0.05, 0]) odom_msg = Odometry() odom_msg.pose.pose.position.x = self.end_point[0] odom_msg.pose.pose.position.y = self.end_point[1] odom_msg.pose.pose.position.z = self.end_point[2] odom_msg.header.stamp = rospy.Time.now() odom_msg.header.frame_id = 'world' self.target_odom.publish(odom_msg) start = time.time() start_point = np.zeros((0,3)) if self.traj_gen_counter != 0: start_point = np.concatenate((start_point, self.p_eoe), 0) # was self.p_eoe else: start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0) # new axis was needed because you dont maintain uniformity points_in_cone = self.generate_regular_pyramid_grid() occupied_points = ros_numpy.numpify(data) #for k in range(len(occupied_points)): # f7 = open('point_cloud.txt', 'a') # f7.write('%s, %s, %s, %s, %s, %s\n' %(self.traj_gen_counter, self.RHP_time, len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2])) #if self.traj_gen_counter == 0: # for k in range(len(occupied_points)): # f7 = open('point_cloud.txt', 'a') # f7.write('%s, %s, %s, %s\n' %(len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2])) if len(occupied_points) != 0: points_in_cone = self.generate_final_grid(points_in_cone, occupied_points) """ points_in_cone = [x[np.newaxis] for x in points_in_cone] pp1 = [np.dot(self.Rcdoc_cdl[0:3, 0:3].T, x.T) for x in points_in_cone] # now translate from visensor frame to cg frame pp2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in pp1] # now rotate the cloud in world frame points_in_cone = [self.Pglobal + np.dot(self.Rglobal.T, x).ravel() for x in pp2] # remove z points if they are going inside the ground, assuming ground is z = 0 points_in_cone = [x for x in points_in_cone if not x[2] <= 0] points_in_cone = np.concatenate((start_point, points_in_cone), 0) """ p1 = [np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone] #p1 = [self.Rvibase_cl[0:3, 3:4] + np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone] p2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in p1] #p2 = [np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in p1] points_in_cone = [self.Pglobal + np.dot(self.Rglobal, x).ravel() for x in p2] points_in_cone = [x for x in points_in_cone if not x[2] <= 0] points_in_cone = np.concatenate((start_point, points_in_cone), 0) # publish generated pointcloud header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' p = pc2.create_cloud_xyz32(header, points_in_cone) self.pcl_pub.publish(p) kdt_points_in_cone = cKDTree(points_in_cone) closest_to_end = kdt_points_in_cone.query(self.end_point, 1) #closest_to_end_index = points_in_cone[closest_to_end[1]] #closest_to_end_point = (closest_to_end_index[0], closest_to_end_index[1], closest_to_end_index[2]) end_point_index = closest_to_end[1] #one dimension simplicial complex which is basically a graph rips = gudhi.RipsComplex(points = points_in_cone, max_edge_length = 1.5*self.resolution) f = rips.create_simplex_tree(max_dimension = 1) # make graph G = nx.Graph() G.add_nodes_from(range(f.num_vertices())) edge_list = [(simplex[0][0], simplex[0][1], {'weight': simplex[1]}) if len(simplex[0])==2 else None for simplex in f.get_skeleton(1)] edge_list = [k for k in edge_list if k is not None] G.add_edges_from(edge_list) try: shortest_path = nx.shortest_path(G, source = 0, target = end_point_index, weight = 'weight', method = 'dijkstra') path = np.array([points_in_cone[j] for j in shortest_path]) except: print 'No path between start and end' #f1 = open('path.txt', 'a') #f1.write('%s, %s\n' %(path, points_in_cone[end_point_index])) #length = nx.shortest_path_length(G, source = 0, target = end_point_index, weight = 'weight', method='dijkstra') # planning horizon is a fixed (5 for now) segments, trajectory will be planned only for these segments, # execution horizon will be just 3 segments # at current resolution of 0.25m, that means a trajecotory of approximately 1.25m will be planned and 0.75m will be executed # at each time the quad plans its motion, depending on how fast it can replan, these values would be changed no_of_segments = 3 no_of_segments_to_track = 1 path = path[:no_of_segments+1] # n segments means n+1 points in the path path = zip(*path) # now construct the minimum snap trajectory on the minimum path waypoint_specified = True waypoint_bc = False T, p1, p2, p3 = Minimum_snap_trajetory(self.uav_velocity, path, waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe, self.j_eoe).construct_polynomial_trajectory() #self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3) N = 8 p1 = [p1[i:i + N] for i in range(0, len(p1), N)] [i.reverse() for i in p1] p2 = [p2[i:i + N] for i in range(0, len(p2), N)] [i.reverse() for i in p2] p3 = [p3[i:i + N] for i in range(0, len(p3), N)] [i.reverse() for i in p3] t = []; xx = []; yy = []; zz = [] vx = []; vy = []; vz = []; accx = []; accy = []; accz = [] jx = []; jy = []; jz = [] #print T traj_frequency = 100 for ii in range(no_of_segments_to_track): #print 'i m here' , ii, len(T) #t.append(np.linspace(T[ii], T[ii+1], int((T[ii+1]-T[ii])*traj_frequency))) t.append(np.linspace(T[ii], T[ii+1], 100)) xx.append(np.poly1d(p1[ii])) vx.append(np.polyder(xx[-1], 1)); accx.append(np.polyder(xx[-1], 2)) jx.append(np.polyder(xx[-1], 3))#; sx.append(np.polyder(xx[-1], 4)) yy.append(np.poly1d(p2[ii])) vy.append(np.polyder(yy[-1], 1)); accy.append(np.polyder(yy[-1], 2)) jy.append(np.polyder(yy[-1], 3))#; sy.append(np.polyder(yy[-1], 4)) zz.append(np.poly1d(p3[ii])) vz.append(np.polyder(zz[-1], 1)); accz.append(np.polyder(zz[-1], 2)) jz.append(np.polyder(zz[-1], 3))#; sz.append(np.polyder(zz[-1], 4)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'frame' traj.header = header traj.joint_names = 'nothing' # testing for now trajectory_start_time = self.RHP_time for i in range(no_of_segments_to_track): # "changed to no_of_segments_to_track" instead of "no_of_segments" for j in t[i]: time.sleep(0.01) self.RHP_time = j + trajectory_start_time xdes = xx[i](j); ydes = yy[i](j); zdes = zz[i](j) vxdes = vx[i](j); vydes = vy[i](j); vzdes = vz[i](j) axdes = accx[i](j); aydes = accy[i](j); azdes = accz[i](j) jxdes = jx[i](j); jydes = jy[i](j); jzdes = jz[i](j) # for now angular acceleration Point msg of MultiDOFJointTrajectory() msg is used to specify the desired direction #vector = np.array([self.end_point[0]-xdes, self.end_point[1]-ydes, self.end_point[2]-zdes]) #vector = np.array([self.p_eoe[0][0]-xdes, self.p_eoe[0][1]-ydes, self.p_eoe[0][2]-zdes]) vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]]) #vector = np.array([self.Vglobal[0], self.Vglobal[1], self.Vglobal[2]]) direction = vector/np.linalg.norm(vector) transforms = Transform(translation = Point(xdes, ydes, zdes), rotation = Quaternion(0,0,0,1)) velocities = Twist(linear = Point(vxdes, vydes, vzdes), angular = Point(0,0,0)) accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(direction[0],direction[1],direction[2])) #accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(1,0,0)) point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(self.RHP_time)) traj.points.append(point) self.uav_traj_pub.publish(traj) traj.points.pop(0) self.p_eoe = np.array([[xdes, ydes, zdes]]) self.v_eoe = np.array([[vxdes, vydes, vzdes]]) self.a_eoe = np.array([[axdes, aydes, azdes]]) self.j_eoe = np.array([[jxdes, jydes, jzdes]]) f1 = open('eoe_points.txt', 'a') f1.write('%s, %s, %s, %s\n' %(self.p_eoe, self.v_eoe, self.a_eoe, self.j_eoe)) #self.uav_traj_pub.publish(traj) time_taken = time.time()-start self.traj_gen_counter += 1 #time.sleep(0.1) #time.sleep(T[no_of_segments_to_track]*0.9-time_taken) f1 = open('total_time_taken.txt', 'a') f1.write("%s\n" % (time_taken)) print 'total time taken to execute the callbacak is:', time_taken
theta_array = [] first_scan = True counter = 0 out_direction_ = 0 OCCUPANCY_GRID_HEIGHT = 3.0 # in meters OCCUPANCY_GRID_WIDTH = 3.0 # in meters RESOLUTION = 8.0 # boxes per meter PIXEL_HEIGHT = int(OCCUPANCY_GRID_HEIGHT*RESOLUTION) PIXEL_WIDTH = int(OCCUPANCY_GRID_WIDTH*RESOLUTION) current_occupancy_grid = np.zeros((PIXEL_WIDTH, PIXEL_HEIGHT)) radius_map = np.zeros((PIXEL_WIDTH, PIXEL_HEIGHT)) theta_map = np.zeros((PIXEL_WIDTH, PIXEL_HEIGHT)) current_odom = Odometry() occupancy_grid_pub = OccupancyGrid() next_point_ = Point() def create_theta_array(scan): theta_min = scan.angle_min theta_max = scan.angle_max theta_delta = (theta_max - theta_min)/len(np.array(scan.ranges)) theta = np.arange(theta_min,theta_max,theta_delta) return theta def create_radius_array(): global radius_map global theta_map for row in range(0, PIXEL_WIDTH): for col in range(0, PIXEL_HEIGHT):
def poll(self): now = rospy.Time.now() if now > self.t_next: #try: # left_pidin, right_pidin = self.arduino.get_pidin() #except: # rospy.logerr("getpidout exception count: ") # return #self.lEncoderPub.publish(left_pidin) #self.rEncoderPub.publish(right_pidin) #try: # left_pidout, right_pidout = self.arduino.get_pidout() #except: # rospy.logerr("getpidout exception count: ") # return #self.lPidoutPub.publish(left_pidout) #self.rPidoutPub.publish(right_pidout) # Read the encoders try: left_enc, right_enc = self.arduino.get_encoder_counts() #rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc)) except: self.bad_encoder_count += 1 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) return try: csb_value1 = self.arduino.get_csb_value() except: rospy.logerr("CSB exception ") try: csb_value2 = self.arduino.get_csb1_value() except: rospy.logerr("CSB exception ") dt = now - self.then self.then = now dt = dt.to_sec() # Calculate odometry if self.enc_left == None: dright = 0 dleft = 0 else: if (left_enc < self.encoder_low_wrap and self.enc_left > self.encoder_high_wrap): self.l_wheel_mult = self.l_wheel_mult + 1 elif (left_enc > self.encoder_high_wrap and self.enc_left < self.encoder_low_wrap): self.l_wheel_mult = self.l_wheel_mult - 1 else: self.l_wheel_mult = 0 if (right_enc < self.encoder_low_wrap and self.enc_right > self.encoder_high_wrap): self.r_wheel_mult = self.r_wheel_mult + 1 elif (right_enc > self.encoder_high_wrap and self.enc_right < self.encoder_low_wrap): self.r_wheel_mult = self.r_wheel_mult - 1 else: self.r_wheel_mult = 0 #dright = (right_enc - self.enc_right) / self.ticks_per_meter #dleft = (left_enc - self.enc_left) / self.ticks_per_meter dleft = 1.0 * (left_enc + self.l_wheel_mult * (self.encoder_max - self.encoder_min) - self.enc_left) / self.ticks_per_meter dright = 1.0 * (right_enc + self.r_wheel_mult * (self.encoder_max - self.encoder_min) - self.enc_right) / self.ticks_per_meter self.enc_right = right_enc self.enc_left = left_enc dxy_ave = (dright + dleft) / 2.0 dth = (dright - dleft) / self.wheel_track vxy = dxy_ave / dt vth = dth / dt if (dxy_ave != 0): dx = cos(dth) * dxy_ave dy = -sin(dth) * dxy_ave self.x += (cos(self.th) * dx - sin(self.th) * dy) self.y += (sin(self.th) * dx + cos(self.th) * dy) if (dth != 0): self.th += dth quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0) # Create the odometry transform frame broadcaster. self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), self.base_frame, "odom") odom = Odometry() odom.header.frame_id = "odom" odom.child_frame_id = self.base_frame odom.header.stamp = now odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = vxy odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth # todo sensor_state.distance == 0 #if self.v_des_left == 0 and self.v_des_right == 0: # odom.pose.covariance = ODOM_POSE_COVARIANCE2 # odom.twist.covariance = ODOM_TWIST_COVARIANCE2 #else: # odom.pose.covariance = ODOM_POSE_COVARIANCE # odom.twist.covariance = ODOM_TWIST_COVARIANCE self.odomPub.publish(odom) self.csbPub.publish(csb_value1) self.csb1Pub.publish(csb_value2) if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): self.v_des_left = 0 self.v_des_right = 0 if self.v_left < self.v_des_left: self.v_left += self.max_accel if self.v_left > self.v_des_left: self.v_left = self.v_des_left else: self.v_left -= self.max_accel if self.v_left < self.v_des_left: self.v_left = self.v_des_left if self.v_right < self.v_des_right: self.v_right += self.max_accel if self.v_right > self.v_des_right: self.v_right = self.v_des_right else: self.v_right -= self.max_accel if self.v_right < self.v_des_right: self.v_right = self.v_des_right self.lVelPub.publish(self.v_left) self.rVelPub.publish(self.v_right) self.csbPub.publish(self.csb_value) # Set motor speeds in encoder ticks per PID loop if not self.stopped: self.arduino.drive(self.v_left, self.v_right) self.t_next = now + self.t_delta
#!/usr/bin/env python # license removed for brevity import rospy import math import tf from sensor_msgs.msg import JointState from std_msgs.msg import Header from nav_msgs.msg import Odometry from geometry_msgs.msg import Quaternion initial_pose = Odometry() target_pose = Odometry() target_distance = 0 def get_pose(initial_pose_tmp): global initial_pose initial_pose = initial_pose_tmp def get_target(target_pose_tmp): global target_pose target_pose = target_pose_tmp def actuator_ctrl(): actuator_vel = 15 return actuator_vel def thruster_ctrl_msg():
from nav_msgs.msg import Odometry import rospy import numpy as np import matplotlib.pyplot as plt import rospkg queue = [] # pub = rospy.Publisher("/gx4_45_imu/data", Imu, queue_size=1) # pub = rospy.Publisher("/pwm_auv_state", Float64[], queue_size=1) count = 0 # r = 0.05 path = rospkg.RosPack().get_path('zeabus_sensor') + '/scripts' rate = 1 / 20. f = open(path + '/auv_state_pwm_00.csv', 'w+') pwm = Pwm() pwm.pwm = [1500] * 8 auv_state = Odometry() imu = Imu() get_data = True pub_pwm = rospy.Publisher("/pwm/sub_sampling", Pwm, queue_size=1) pub_imu = rospy.Publisher("/imu/sub_sampling", Imu, queue_size=1) pub_state = rospy.Publisher("/state/sub_sampling", Odometry, queue_size=1) def change_rate(): global rate, auv_state, pwm, count, f, get_data, imu pub_imu.publish(imu) pub_pwm.publish(pwm) pub_state.publish(auv_state) string = str(imu.linear_acceleration.x) + ", " + str( imu.linear_acceleration.y) + "\n"
frame_R = np.zeros((480, 640, 1), np.uint8) frame_L_prev = np.zeros((480, 640, 1), np.uint8) frame_R_prev = np.zeros((480, 640, 1), np.uint8) #ros Images left_image = Image() right_image = Image() left_featured_image = Image() right_featured_image = Image() matched_featured_image = Image() flow_left_matched_featured_image = Image() flow_left_image = Image() debug_image = Image() #relative pose pose_rel = Odometry() def pose_estimation(): global f, B global frame_L, frame_R, frame_L_prev, frame_R_prev global left_image, right_image, img_L, img_R global left_featured_image, right_featured_image, matched_featured_image global img_L, frame_L, frame_L_prev, dt_L global flow_left_matched_featured_image, flow_left_image global height, width, scale frame_L_prev = frame_L frame_R_prev = frame_R try:
def poll(self): now = rospy.Time.now() if now > self.t_next: # Read the encoders try: self.diagnostics.reads += 1 self.diagnostics.total_reads += 1 left_enc, right_enc = self.arduino.get_encoder_counts() self.diagnostics.freq_diag.tick() except: self.diagnostics.errors += 1 self.bad_encoder_count += 1 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) return # Check for jumps in encoder readings if self.detect_enc_jump_error: try: #rospy.loginfo("Left: %d LEFT: %d Right: %d RIGHT: %d", left_enc, self.enc_left, right_enc, self.enc_right) enc_jump_error = False if abs(right_enc - self.enc_right) > self.enc_jump_error_threshold: self.diagnostics.errors += 1 self.bad_encoder_count += 1 rospy.logerr("RIGHT encoder jump error from %d to %d", self.enc_right, right_enc) self.enc_right = right_enc enc_jump_error = True if abs(left_enc - self.enc_left) > self.enc_jump_error_threshold: self.diagnostics.errors += 1 self.bad_encoder_count += 1 rospy.logerr("LEFT encoder jump error from %d to %d", self.enc_left, left_enc) self.enc_left = left_enc enc_jump_error = True if enc_jump_error: return except: pass dt = now - self.then self.then = now dt = dt.to_sec() # Calculate odometry if self.enc_left == None: dright = 0 dleft = 0 else: dright = (right_enc - self.enc_right) / self.ticks_per_meter dleft = (left_enc - self.enc_left) / self.ticks_per_meter self.enc_right = right_enc self.enc_left = left_enc dxy_ave = self.odom_linear_scale_correction * (dright + dleft) / 2.0 dth = self.odom_angular_scale_correction * ( dright - dleft) / float(self.wheel_track) vxy = dxy_ave / dt vth = dth / dt if (dxy_ave != 0): dx = cos(dth) * dxy_ave dy = -sin(dth) * dxy_ave self.x += (cos(self.th) * dx - sin(self.th) * dy) self.y += (sin(self.th) * dx + cos(self.th) * dy) if (dth != 0): self.th += dth quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0) # Create the odometry transform frame broadcaster. if self.publish_odom_base_transform: self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), self.base_frame, "odom") odom = Odometry() odom.header.frame_id = "odom" odom.child_frame_id = self.base_frame odom.header.stamp = now odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = vxy odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth self.current_speed = Twist() self.current_speed.linear.x = vxy self.current_speed.angular.z = vth """ Covariance values taken from Kobuki node odometry.cpp at: https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/odometry.cpp Pose covariance (required by robot_pose_ekf) TODO: publish realistic values Odometry yaw covariance must be much bigger than the covariance provided by the imu, as the later takes much better measures """ odom.pose.covariance[0] = 0.1 odom.pose.covariance[7] = 0.1 if self.use_imu_heading: #odom.pose.covariance[35] = 0.2 odom.pose.covariance[35] = 0.05 else: odom.pose.covariance[35] = 0.05 odom.pose.covariance[ 14] = sys.float_info.max # set a non-zero covariance on unused odom.pose.covariance[ 21] = sys.float_info.max # dimensions (z, pitch and roll); this odom.pose.covariance[ 28] = sys.float_info.max # is a requirement of robot_pose_ekf self.odomPub.publish(odom) if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): self.v_des_left = 0 self.v_des_right = 0 #self.v_left = 0 #self.v_right = 0 if self.v_left < self.v_des_left: self.v_left += self.max_accel if self.v_left > self.v_des_left: self.v_left = self.v_des_left else: self.v_left -= self.max_accel if self.v_left < self.v_des_left: self.v_left = self.v_des_left if self.v_right < self.v_des_right: self.v_right += self.max_accel if self.v_right > self.v_des_right: self.v_right = self.v_des_right else: self.v_right -= self.max_accel if self.v_right < self.v_des_right: self.v_right = self.v_des_right # Set motor speeds in encoder ticks per PID loop if not self.stopped: rospy.loginfo("left: {0}".format(self.v_left)) rospy.loginfo("des_left: {0}".format(self.v_des_left)) self.arduino.drive(self.scale(self.v_left), self.scale(self.v_right)) self.t_next = now + self.t_delta
from std_msgs.msg import String, Float32, UInt16 from sensor_msgs.msg import Imu, JointState, Joy from geometry_msgs.msg import Vector3Stamped, Twist from nav_msgs.msg import Odometry from DecayFilter import DecayFilter import csv CONFIG_ENABLE_LOG = False CONFIG_DEBUG_PUBLISHER = False CONFIG_DEBUG_PRINT_DATA_SOURCE = True CONFIG_TOPIC_WHEELODOM = '/ak1/odom' CONFIG_TOPIC_REFODOM = '/vive/LHR_0EB0243A_odom' # ak1: /vive/LHR_0EB0243A_odom, ak2: /vive/LHR_08DF7BFF_odom ak_wheelodom = Odometry() ak_refodom = Odometry() def normpdf(x, mu, sigma2): return np.exp(-(x - mu)**2 / (2 * sigma2)) / np.sqrt(2 * np.pi * sigma2) def Pr_L_consistent(L): return normpdf(L, 0, 0.08947) * 2 def Pr_L_diverged(L): return normpdf(L, 0.40272, 0.10208)
def auto_demo(): rospy.init_node('autonomous_demo') #Requirements pub1 = rospy.Publisher('/gps/fix', NavSatFix, queue_size =10) pub2 = rospy.Publisher('/imu/data', Imu ,queue_size = 10) pub3 = rospy.Publisher('/odometry/wheel', Odometry, queue_size = 10) pub4 = rospy.Publisher('/gps_waypoint_handler/status', String, queue_size = 10) #Autonomous movement #pub5 = rospy.Publisher('/move_base/status',GoalStatusArray,queue_size=10) #Image Detect pub6 = rospy.Publisher('/image_detect_topic',String,queue_size=10) #Image Reach pub7 = rospy.Publisher('/image_reach_topic',String,queue_size=10) rate = rospy.Rate(10) # 10hz count = 0 while not rospy.is_shutdown(): print("0: All sensors are good.") print("1: All sensors except encoder are good.") print("2: Damaged Sensors") print("3: Got Waypoint") print("4: Did not get any waypoint") print("5: Reached to way point") print("6: Did not Reached to way point") print("7: Detected Image") print("8: Did not detect Image") print("9: Reached Image") print("10: Did not reached image") imuMsg = Imu() imuMsg.orientation.x = 5 imuMsg.orientation.y = 5 gpsMsg = NavSatFix() gpsMsg.latitude = 5 gpsMsg.longitude = 5 encoderMsg = Odometry() encoderMsg.pose.pose.position.x = 5 encoderMsg.pose.pose.position.y = 5 wpStatusMsgLow = GoalStatus() wpStatusMsgLow.status = 3 wpStatusArray =[] wpStatusArray.append(wpStatusMsgLow) wpStatusMsg = GoalStatusArray() wpStatusMsg.status_list = wpStatusArray userInput = raw_input() if userInput == "0": #All sensors are good. pub1.publish(gpsMsg) pub2.publish(imuMsg) pub3.publish(encoderMsg) elif userInput == "1": # All sensors except encoder are good. pub1.publish(gpsMsg) pub2.publish(imuMsg) #pub3.publish("0") elif userInput == "2": #Damaged Sensors pub1.publish(gpsMsg) #pub2.publish("0") pub3.publish(encoderMsg) elif userInput == "3": # Got Waypoint pub4.publish("1") #elif userInput == "4": #Did not get any waypoint #pub4.publish("0") elif userInput == "5": #Reached to way point #pub5.publish(wpStatusMsg) pub4.publish("2") #elif userInput == "6": #Did not reached to way point #pub5.publish("0") elif userInput == "7": #Detected Image pub6.publish("1") elif userInput == "8": #Did not Detect Image pub6.publish("0") elif userInput == "9": #Reached Image pub7.publish("1") elif userInput == "10": #Did not reached image pub7.publish("0") else: print("Invalid entry") rate.sleep();
def pose_update(self, rmp): """ Read in the current RMP feedback and publish the pose :param rmp: the RMP feedback message """ rmp_items = rmp.sensor_items rmp_values = rmp.sensor_values time_msg_received = rmp.header.stamp.secs + 10**-9 * rmp.header.stamp.nsecs """ get the values for the feedback items needed angles and angle rates are flipped because the RMP's do not follow the right hand rule convention """ for x in range(0, len(rmp_items)): if rmp_items[x] == 'linear_vel_mps': lin_vel = rmp_values[x] elif rmp_items[x] == 'linear_pos_m': lin_pos = rmp_values[x] elif rmp_items[x] == 'differential_wheel_vel_rps': diff_rate = -1 * rmp_values[x] elif rmp_items[x] == 'angle_target_deg': ang_targ = (rmp_values[x] - 90) * math.pi / 180 elif rmp_items[x] == 'pse_pitch_deg': pitch = (-1 * rmp_values[x]) * math.pi / 180 elif rmp_items[x] == 'pse_roll_deg': roll = (-1 * rmp_values[x]) * math.pi / 180 """ Segway RMP base tends to drift when stationary, basic filter to ignore low differences and reduce/stop drift """ if diff_rate >= -0.005 and diff_rate <= 0.005: diff_rate = 0.0 """ Calculate the new pose based on the feedback from the Segway and the time difference from the previous calculation """ yaw = self.prev_yaw + diff_rate * (time_msg_received - self.prev_time) x_pos = self.prev_x_pos + (lin_pos - self.prev_lin_pos) * math.cos(yaw) y_pos = self.prev_y_pos + (lin_pos - self.prev_lin_pos) * math.sin(yaw) # store the current values to be used in the next iteration self.prev_lin_pos = lin_pos self.prev_x_pos = x_pos self.prev_y_pos = y_pos self.prev_yaw = yaw self.prev_time = time_msg_received # create quaternion array from rmp and IMU data quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) # make and publish the odometry message odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" odom.child_frame_id = "base_footprint" odom.pose.pose.position.x = x_pos odom.pose.pose.position.y = y_pos odom.pose.pose.position.z = 0.0 odom.pose.pose.orientation.x = quaternion[0] odom.pose.pose.orientation.y = quaternion[1] odom.pose.pose.orientation.z = quaternion[2] odom.pose.pose.orientation.w = quaternion[3] odom.twist.twist.linear.x = lin_vel * math.cos(ang_targ) odom.twist.twist.linear.y = lin_vel * math.sin(ang_targ) odom.twist.twist.angular.z = diff_rate self.odomPub.publish(odom) # publish the transform from odom to the base footprint if self.publish_tf: self.tfBroadCast.sendTransform((x_pos, y_pos, 0.0), quaternion, rospy.Time.now(), '/base_footprint', '/odom')
def __init__(self, parent=None): super(PaintWidget, self).__init__(parent) # geometry parameters self.geometry = Geometry() self.scaleOnField = 1.0 self.world_height = 0.0 self.world_width = 0.0 self.trans = QPointF(0.0, 0.0) # 慢性的なトランス self.mouseTrans = QPointF(0.0, 0.0) # マウス操作で発生する一時的なトランス self.scale = QPointF(1.0, 1.0) self.clickPoint = QPointF(0.0, 0.0) self._current_mouse_pos = QPointF(0.0, 0.0) # Colors self.friendDrawColor = Qt.cyan self.enemyDrawColor = Qt.yellow self.friend_color = rospy.get_param("friend_color", "blue") if self.friend_color != "blue": self.friendDrawColor = Qt.yellow self.enemyDrawColor = Qt.cyan self.targetPosDrawColor = QColor(102, 0, 255, 100) self.avoidingPointDrawColor = QColor(255, 0, 0, 100) # Replace self._CLICK_POS_THRESHOLD = 0.1 self._CLICK_VEL_ANGLE_THRESHOLD = self._CLICK_POS_THRESHOLD + 0.1 self._VEL_GAIN = 3.0 self._VEL_MAX = 8.0 self._replace_func = None self._replace_id = 0 self._replace_is_yellow = False self._replace_is_friend = False # Status self._should_rotate_world = False self._can_replace = False self._is_ballpos_replacement = False self._is_ballvel_replacement = False self._is_robotpos_replacement = False self._is_robotangle_replacement = False # Configs # This function enables mouse tracking without pressing mouse button self.setMouseTracking(True) # Subscribers self.field_geometry = GeometryFieldSize() self.sub_geometry = rospy.Subscriber("geometry_field_size", GeometryFieldSize, self.callbackGeometry) self.ballOdom = Odometry() self.sub_ballPosition = rospy.Subscriber("ball_observer/estimation", Odometry, self.callbackBallOdom) self.friendsIDArray = UIntArray() self.sub_friendsID = rospy.Subscriber("existing_friends_id", UIntArray, self.callbackFriendsID) self._ID_MAX = rospy.get_param('id_max', 12) self.friendOdoms = [Odometry()] * self._ID_MAX self.sub_friendOdoms = [] self.enemyIDArray = UIntArray() self.sub_enemiesID = rospy.Subscriber("existing_enemies_id", UIntArray, self.callbackEnemiesID) self.refereeBranch = String() self.sub_referee_branch = rospy.Subscriber("referee_branch", String, self.callbackRefereeBranch) self.passTargetPointFrameCount = 0 self.passTargetPoint = Point() self.sub_passTargetPoint = rospy.Subscriber( "pass_target_point", Point, self.callbackPassTargetPoint) self.debugPoint = [Point()] * 10 rospy.Subscriber("debug_point_0", Point, self.callbackDebugPoint0) rospy.Subscriber("debug_point_1", Point, self.callbackDebugPoint1) rospy.Subscriber("debug_point_2", Point, self.callbackDebugPoint2) rospy.Subscriber("debug_point_3", Point, self.callbackDebugPoint3) rospy.Subscriber("debug_point_4", Point, self.callbackDebugPoint4) rospy.Subscriber("debug_point_5", Point, self.callbackDebugPoint5) rospy.Subscriber("debug_point_6", Point, self.callbackDebugPoint6) rospy.Subscriber("debug_point_7", Point, self.callbackDebugPoint7) rospy.Subscriber("debug_point_8", Point, self.callbackDebugPoint8) rospy.Subscriber("debug_point_9", Point, self.callbackDebugPoint9) self.enemyOdoms = [Odometry()] * self._ID_MAX self.sub_enemyOdoms = [] self.targetPositions = [PoseStamped()] * self._ID_MAX self.sub_targetPositions = [] self.targetVelocities = [TwistStamped()] * self._ID_MAX self.sub_targetVelocities = [] self.targetIsPosition = [False] * self._ID_MAX self.avoidingPoints = [Point()] * self._ID_MAX self.sub_avoidingPoints = [] for i in range(self._ID_MAX): strID = str(i) topicFriend = "robot_" + strID + "/odom" topicEnemy = "enemy_" + strID + "/odom" topicPosition = "robot_" + strID + "/move_base_simple/goal" topicVelocity = "robot_" + strID + "/move_base_simple/target_velocity" topicAvoidingPoint = "robot_" + strID + "/avoiding_point" self.sub_friendOdoms.append( rospy.Subscriber(topicFriend, Odometry, self.callbackFriendOdom, callback_args=i)) self.sub_enemyOdoms.append( rospy.Subscriber(topicEnemy, Odometry, self.callbackEnemiesOdom, callback_args=i)) self.sub_targetPositions.append( rospy.Subscriber(topicPosition, PoseStamped, self.callbackTargetPosition, callback_args=i)) self.sub_targetVelocities.append( rospy.Subscriber(topicVelocity, TwistStamped, self.callbackTargetVelocity, callback_args=i)) self.sub_avoidingPoints.append( rospy.Subscriber(topicAvoidingPoint, Point, self.callbackAvoidingPoint, callback_args=i)) # Publishers self._pub_replace_ball = rospy.Publisher('replacement_ball', ReplaceBall, queue_size=10) self._pub_replace_robot = rospy.Publisher('replacement_robot', ReplaceRobot, queue_size=10) self.resizeDrawWorld()
def DataReader(): data = ReadFile(FILE_NAME) # IMU imu_msg = Imu() imu_msg.orientation.x = data[0][1:-1,0] imu_msg.orientation.y = data[0][1:-1,1] imu_msg.orientation.z = data[0][1:-1,2] imu_msg.orientation.w = data[0][1:-1,3] imu_msg.linear_acceleration.x=data[0][1:-1,4] imu_msg.linear_acceleration.y=data[0][1:-1,5] imu_msg.linear_acceleration.z=data[0][1:-1,6] imu_msg.angular_velocity.x=data[0][1:-1,7] imu_msg.angular_velocity.y=data[0][1:-1,8] imu_msg.angular_velocity.z=data[0][1:-1,9] imu_time = data[0][1:-1,10] imu_length = len(imu_msg.orientation.x) imu_roll_angle=np.zeros(imu_length,dtype=float) imu_pitch_angle=np.zeros(imu_length,dtype=float) imu_yaw_angle=np.zeros(imu_length,dtype=float) for i in range(imu_length): x = imu_msg.orientation.x[i] y = imu_msg.orientation.y[i] z = imu_msg.orientation.z[i] w = imu_msg.orientation.w[i] imu_roll_angle[i] = math.atan2(2 * (y*z + w*x), w*w - x*x - y*y + z*z)*180/PI imu_pitch_angle[i] = math.asin(-2 * (x*z - w*y))*180/PI imu_yaw_angle[i] = math.atan2(2 * (x*y + w*z), w*w + x*x - y*y - z*z)*180/PI # vehicle_state vehicle_msg =Odometry() vehicle_msg.pose.pose.position.x =data[1][1:-1,0] vehicle_msg.pose.pose.position.y =data[1][1:-1,1] vehicle_msg.pose.pose.position.z =data[1][1:-1,2] vehicle_msg.pose.pose.orientation.x = data[1][1:-1,3] vehicle_msg.pose.pose.orientation.y = data[1][1:-1,4] vehicle_msg.pose.pose.orientation.z = data[1][1:-1,5] vehicle_msg.pose.pose.orientation.w = data[1][1:-1,6] vehicle_msg.twist.twist.linear.x= data[1][1:-1,7] vehicle_msg.twist.twist.linear.y= data[1][1:-1,8] vehicle_msg.twist.twist.linear.z= data[1][1:-1,9] vehicle_msg.twist.twist.angular.x= data[1][1:-1,10] vehicle_msg.twist.twist.angular.y= data[1][1:-1,11] vehicle_msg.twist.twist.angular.z= data[1][1:-1,12] vehicle_time = data[1][1:-1,13] odom_length = len(vehicle_msg.pose.pose.orientation.x) odom_roll_angle=np.zeros(odom_length,dtype=float) odom_pitch_angle=np.zeros(odom_length,dtype=float) odom_yaw_angle=np.zeros(odom_length,dtype=float) for i in range(odom_length): x = vehicle_msg.pose.pose.orientation.x[i] y = vehicle_msg.pose.pose.orientation.y[i] z = vehicle_msg.pose.pose.orientation.z[i] w = vehicle_msg.pose.pose.orientation.w[i] odom_roll_angle[i] = math.atan2(2 * (y*z + w*x), w*w - x*x - y*y + z*z)*180/PI odom_pitch_angle[i] = math.asin(-2 * (x*z - w*y))*180/PI odom_yaw_angle[i] = math.atan2(2 * (x*y + w*z), w*w + x*x - y*y - z*z)*180/PI # GPS pos_gps = NavSatFix() pos_gps.latitude = data[2][1:-1,1] pos_gps.longitude = data[2][1:-1,2] pos_gps.altitude = data[2][1:-1,3] gps_time = data[2][1:-1,4] # chassis chassis_msg_Steer = data[3][1:-1,0] chassis_msg_Throttle = data[3][1:-1,1] chassis_msg_Brake = data[3][1:-1,2] chassis_msg_Wheel = data[3][1:-1,3] chassis_msg_Speed = data[3][1:-1,4] chassis_msg_BucketAngle = data[3][1:-1,5] chassis_msg_EngineSpeed = data[3][1:-1,6] chassis_msg_EngineTorque = data[3][1:-1,7] chassis_time = data[3][1:-1,8] plt.figure(1) loc='center' font_dict={'fontsize': 20,\ 'fontweight' : 8.2,\ 'verticalalignment': 'baseline',\ 'horizontalalignment': loc} plt.title('trajectory',fontdict=font_dict,loc=loc) plt.plot(pos_gps.latitude,pos_gps.longitude, color='cyan', label='desired_trajectory') plt.xlabel('latitude') plt.ylabel('longitude') plt.figure(2) loc='center' font_dict={'fontsize': 20,\ 'fontweight' : 8.2,\ 'verticalalignment': 'baseline',\ 'horizontalalignment': loc} plt.title('Pitch Angle Comparison',fontdict=font_dict,loc=loc) plt.plot(vehicle_time,odom_pitch_angle, color='cyan', label='pitch angle from odom') plt.plot(imu_time,-imu_pitch_angle, color='r', label='pitch angle from imu') plt.legend() plt.ylabel('pitch angle/degree') plt.xlabel('time/s') plt.figure(3) loc='center' font_dict={'fontsize': 20,\ 'fontweight' : 8.2,\ 'verticalalignment': 'baseline',\ 'horizontalalignment': loc} plt.title('trajectory',fontdict=font_dict,loc=loc) plt.plot(vehicle_msg.pose.pose.position.x ,vehicle_msg.pose.pose.position.y, color='cyan', label='desired_trajectory') plt.xlabel('X') plt.ylabel('Y') plt.show()
def __init__(self): #init stuff #get stuff self.num_landmarks = 12 # Estimator stuff # x = pn, pe, pd, phi, theta, psi self.xhat = np.zeros((9 + 3*self.num_landmarks, 1)) self.xhat_odom = Odometry() # Covariance matrix self.P = np.zeros((9 + 3*self.num_landmarks, 9 + 3*self.num_landmarks)) self.P[9:,9:] = np.eye(3*self.num_landmarks)*9999999.9 # Inf self.Q = np.diag([10.0, 5.0, 5.0]) # meas noise # Measurements stuff # Truth self.truth_pn = 0.0 self.truth_pe = 0.0 self.truth_pd = 0.0 self.truth_phi = 0.0 self.truth_theta = 0.0 self.truth_psi = 0.0 self.truth_p = 0.0 self.truth_q = 0.0 self.truth_r = 0.0 self.truth_pndot = 0.0 self.truth_pedot = 0.0 self.truth_pddot = 0.0 self.truth_u = 0.0 self.truth_v = 0.0 self.truth_w = 0.0 self.prev_time = 0.0 self.imu_az = 0.0 #aruco Stuff self.aruco_location = { 76 :[-0.51, 2.302, -1.31], 245 :[1.455, 2.493, -1.486], 55 :[3.92, 1.333, -1.498], 110 :[3.964, -1.753,-1.566], 248 :[2.916, -2.543,-1.537], 64 :[1.181, -2.471,-1.581], 25 :[-1.593,-2.488,-1.572], 121 :[-3.528,-0.658,-1.461], 5 :[-2.023, 2.462,-1.492], } self.landmark_number = { 76:[1], 245:[2], 55:[3], 110:[4], 248:[5], 64:[6], 25:[7], 121:[8], 5:[9], } # Number of propagate steps self.N = 5 #Constants self.g = 9.8 # ROS Stuff # Init subscribers self.truth_sub_ = rospy.Subscriber('/mocap/thor/pose', PoseStamped, self.truth_callback) self.imu_sub_ = rospy.Subscriber('/imu/data', Imu, self.imu_callback) self.velocity_sub_ = rospy.Subscriber('/velocities', Vector3Stamped, self.velocity_callback) self.aruco_sub = rospy.Subscriber('/aruco/measurements', MarkerMeasurementArray, self.aruco_meas_callback ) # Init publishers self.estimate_pub_ = rospy.Publisher('/ekf_estimate', Odometry, queue_size=10) # # Init Timer self.pub_rate_ = 100. # self.update_timer_ = rospy.Timer(rospy.Duration(1.0/self.pub_rate_), self.pub_est)
def main(): global img_ball global img_arr global xs global zs global pred_xs global pred_zs rospy.init_node("colorOfBall") image_sub = rospy.Subscriber("/camera/color/image_raw",Image,imageCallback) depth_sub = rospy.Subscriber("/camera/depth/image_rect_raw",Image,depthCallback) ballPos_pub = rospy.Publisher("/ball/position",Odometry,queue_size=10) print("before wait") rospy.wait_for_message("/camera/color/image_raw",Image) rospy.wait_for_message("/camera/depth/image_rect_raw",Image) print("after wait") rate = rospy.Rate(20) pixToDegree = float(58)/640 while not rospy.is_shutdown(): rospy.wait_for_message("/camera/color/image_raw",Image) rospy.wait_for_message("/camera/depth/image_rect_raw",Image) cv_image_hsv = cv2.cvtColor(img_ball,cv2.COLOR_BGR2HSV) #(rows,cols,channels) = img_ball.shape #print([rows,cols,channels]) #cv2.circle(cv_image,(320,240),10,(255,0,0)) mask = cv2.inRange(cv_image_hsv,orange_lower,orange_upper) mask = cv2.erode(mask,None,iterations=1) mask = cv2.dilate(mask,None,iterations=1) # find contours in the mask and initialize the current # (x, y) center of the ball cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) center = None # only proceed if at least one contour was found x = 0 z = 0 if len(cnts) > 0: # find the largest contour in the mask, then use # it to compute the minimum enclosing circle and # centroid c = max(cnts, key=cv2.contourArea) ((img_ballx, img_bally), radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) z = toDistance(float(img_arr[int(img_bally),int(img_ballx)])) #print(float(img_arr[img_bally,img_ballx])) # only proceed if the radius meets a minimum size if radius > 5 and radius < 200 and img_bally > 140 and (img_bally<380 and img_ballx<540): # draw the circle and centroid on the frame, # then update the list of tracked points cv2.circle(img_ball, (int(img_ballx), int(img_bally)), int(radius), (0, 0, 255), 2) cv2.circle(mask,(int(img_ballx), int(img_bally)), int(radius), (0, 0, 255), 2) #print([int(img_ballx),int(img_bally),int(radius),z]) deg = pixToDegree*float(img_ballx - 320) x = z*np.tan(deg*pi/180) pos = Odometry() if x!=0 and z!=0: print([x,z]) xs = np.append(xs,x) zs = np.append(zs,z) pos.pose.pose.position.x = x pos.pose.pose.position.z = z ballPos_pub.publish(pos) cv2.imshow("Image window",img_ball) cv2.imshow("Mask",mask) #print([0,0,0] + cv_image_hsv[320,240,:]) cv2.waitKey(1) & 0xFF if xs.size > 20: xs = xs[-19:] if zs.size > 20: zs = zs[-19:] if xs.size > 10: pred_zs = findLine(xs[-10:],zs[-10:]) #plt.plot(xs,zs) #plt.plot(x,z,'x',markersize=12) if show_ball: marks.set_xdata(xs) marks.set_ydata(zs) current_mark.set_xdata(x) current_mark.set_ydata(z) if show_pred_path: pred_line.set_ydata(pred_zs) plt.draw() plt.show() rate.sleep() cv2.destroyAllWindows()
def spin(self): encoders = [0, 0] self.x = 0 # position in xy plane self.y = 0 self.th = 0 then = rospy.Time.now() # things that don't ever change scan_link = rospy.get_param('~frame_id', 'base_laser_link') scan = LaserScan(header=rospy.Header(frame_id=scan_link)) scan.angle_min = 0 scan.angle_max = 6.26 scan.angle_increment = 0.017437326 scan.range_min = 0.020 scan.range_max = 5.0 odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link') # main loop of driver r = rospy.Rate(5) self.robot.requestScan() while not rospy.is_shutdown(): # prepare laser scan scan.header.stamp = rospy.Time.now() #self.robot.requestScan() scan.ranges = self.robot.getScanRanges() # get motor encoder values left, right = self.robot.getMotors() # send updated movement commands self.robot.setMotors( self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1]))) # ask for the next scan while we finish processing stuff self.robot.requestScan() # now update position information dt = (scan.header.stamp - then).to_sec() then = scan.header.stamp d_left = (left - encoders[0]) / 1000.0 d_right = (right - encoders[1]) / 1000.0 encoders = [left, right] dx = (d_left + d_right) / 2 dth = (d_right - d_left) / (BASE_WIDTH / 1000.0) x = cos(dth) * dx y = -sin(dth) * dx self.x += cos(self.th) * x - sin(self.th) * y self.y += sin(self.th) * x + cos(self.th) * y self.th += dth # prepare tf from base_link to odom quaternion = Quaternion() quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0) # prepare odometry odom.header.stamp = rospy.Time.now() odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = dx / dt odom.twist.twist.angular.z = dth / dt # publish everything self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then, "base_link", "odom") self.scanPub.publish(scan) self.odomPub.publish(odom) # wait, then do it again r.sleep() # shut down self.robot.setLDS("off") self.robot.setTestMode("off")
#! /usr/bin/env python import rospy from audibot_cfg.vehicle_ns import AudiBotNS from std_msgs.msg import Header from nav_msgs.msg import Odometry from geometry_msgs.msg import TwistStamped from gazebo_msgs.srv import GetModelState, GetModelStateRequest audibot_obj = AudiBotNS() audibot_obj.parse_args() audibot_obj.exec_odom_cfg() node_name, model_name, frame_id, topic_name = audibot_obj.get_odom_cfg() rospy.init_node(node_name, anonymous=True) odom_pub = rospy.Publisher(topic_name, Odometry, queue_size=10) rospy.wait_for_service('/gazebo/get_model_state') get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) odom = Odometry() header = Header() header.frame_id = frame_id model = GetModelStateRequest() model.model_name = model_name r = rospy.Rate(10) while not rospy.is_shutdown(): result = get_model_srv(model) odom.pose.pose = result.pose odom.twist.twist = result.twist header.stamp = rospy.Time.now() odom.header = header odom_pub.publish(odom) r.sleep()
from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from sensor_msgs.msg import Range from std_msgs.msg import Int32 from bebop_msgs.msg import Ardrone3PilotingStateAltitudeChanged from bebop_msgs.msg import Ardrone3PilotingStateAttitudeChanged h_p = 0 x_p = 0 y_p = 0 z_o = 0 cnt = 0 mx = 0 my = 0 odom_var = Odometry() def receive_odom(data): global x_p, y_p x_p = data.pose.pose.position.x y_p = data.pose.pose.position.y def cnt_callback(data): global cnt cnt = data.data def my_callback(data): global my
def odometry_calculation(self): if self.skid_steer: ### Skid-Steering model velocity_left, velocity_right = espeleo.left_right_velocity([ self.motor_velocity1, self.motor_velocity2, self.motor_velocity3, self.motor_velocity4, self.motor_velocity5, self.motor_velocity6 ]) # Linear velocity v_robot = (velocity_right + velocity_left) * (self.alpha_skid / 2) # Angular velocity w_robot = ((velocity_right - velocity_left) * (self.alpha_skid / (2 * self.ycir_skid))) if self.time_counter_aux == 0: self.last_time = self.current_time self.time_counter_aux = 1 # Velocity in the XY plane vx_robot = v_robot * cos(self.th) vy_robot = v_robot * sin(self.th) # Calculating odometry dt = self.current_time - self.last_time delta_x = vx_robot * dt delta_y = vy_robot * dt delta_th = w_robot * dt # Integrating pose self.x += delta_x self.y += delta_y self.th += delta_th else: v_robot, w_robot = espeleo.get_espeleo_velocity([ self.motor_velocity1, self.motor_velocity2, self.motor_velocity3, self.motor_velocity4, self.motor_velocity5, self.motor_velocity6 ]) # Velocity in the XY plane vx_robot = v_robot * cos(self.th) vy_robot = v_robot * sin(self.th) if self.time_counter_aux == 0: self.last_time = self.current_time self.time_counter_aux = 1 # Calculating odometry dt = self.current_time - self.last_time delta_x = vx_robot * dt delta_y = vy_robot * dt delta_th = w_robot * dt # Integrating pose self.x += delta_x self.y += delta_y self.th += delta_th vel_robot = Twist() vel_robot.linear.x = v_robot vel_robot.angular.z = w_robot # Since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.th) # First, we'll publish the transform over tf self.odom_broadcaster.sendTransform( (self.x, self.y, 0.), odom_quat, rospy.Time.now(), "base_link", "odom" # odom ) # next, we'll publish the odometry message over ROS odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" # set the position odom.pose.pose = Pose(Point(self.x, self.y, 0.), Quaternion(*odom_quat)) # set the velocity odom.child_frame_id = "base_link" odom.twist.twist = Twist(Vector3(v_robot, 0, 0), Vector3(0, 0, w_robot)) # Calculating the covariance based on the angular velocity # if the robot is rotating, the covariance is higher than if its going straight if abs(w_robot) > 0.2: covariance_cons = 0.3 else: covariance_cons = 0.05 odom.pose.covariance[0] = covariance_cons odom.pose.covariance[7] = covariance_cons odom.pose.covariance[35] = 100 * covariance_cons odom.pose.covariance[1] = covariance_cons odom.pose.covariance[6] = covariance_cons odom.pose.covariance[31] = covariance_cons odom.pose.covariance[11] = covariance_cons odom.pose.covariance[30] = 10 * covariance_cons odom.pose.covariance[5] = 10 * covariance_cons odom.pose.covariance[14] = 0.1 odom.pose.covariance[21] = 0.1 odom.pose.covariance[28] = 0.1 odom.twist.covariance[0] = covariance_cons odom.twist.covariance[7] = covariance_cons odom.twist.covariance[35] = 100 * covariance_cons odom.twist.covariance[1] = covariance_cons odom.twist.covariance[6] = covariance_cons odom.twist.covariance[31] = covariance_cons odom.twist.covariance[11] = covariance_cons odom.twist.covariance[30] = 10 * covariance_cons odom.twist.covariance[5] = 10 * covariance_cons odom.twist.covariance[14] = 0.1 odom.twist.covariance[21] = 0.1 odom.twist.covariance[28] = 0.1 # publish the message self.odom_pub.publish(odom) self.vel_pub.publish(vel_robot) self.last_time = self.current_time
def spin(self): encoders = [0, 0] self.x = 0 # position in xy plane self.y = 0 self.th = 0 then = rospy.Time.now() # things that don't ever change scan_link = rospy.get_param('~frame_id', 'base_laser_link') scan = LaserScan(header=rospy.Header(frame_id=scan_link)) scan.angle_min = 0.0 scan.angle_max = 359.0 * pi / 180.0 scan.angle_increment = pi / 180.0 scan.range_min = 0.020 scan.range_max = 5.0 odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint') button = Button() sensor = Sensor() self.robot.setBacklight(1) self.robot.setLED("Green") # main loop of driver r = rospy.Rate(20) cmd_rate = self.CMD_RATE while not rospy.is_shutdown(): # notify if low batt #if self.robot.getCharger() < 10: # print "battery low " + str(self.robot.getCharger()) + "%" # get motor encoder values left, right = self.robot.getMotors() cmd_rate = cmd_rate - 1 if cmd_rate == 0: # send updated movement commands #if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]: # max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1]))) #self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2) self.robot.setMotors( self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1]))) cmd_rate = self.CMD_RATE self.old_vel = self.cmd_vel # prepare laser scan scan.header.stamp = rospy.Time.now() self.robot.requestScan() scan.ranges, scan.intensities = self.robot.getScanRanges() # now update position information dt = (scan.header.stamp - then).to_sec() then = scan.header.stamp d_left = (left - encoders[0]) / 1000.0 d_right = (right - encoders[1]) / 1000.0 encoders = [left, right] #print d_left, d_right, encoders dx = (d_left + d_right) / 2 dth = (d_right - d_left) / (self.robot.base_width / 1000.0) x = cos(dth) * dx y = -sin(dth) * dx self.x += cos(self.th) * x - sin(self.th) * y self.y += sin(self.th) * x + cos(self.th) * y self.th += dth #print self.x,self.y,self.th # prepare tf from base_link to odom quaternion = Quaternion() quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0) # prepare odometry odom.header.stamp = rospy.Time.now() odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = dx / dt odom.twist.twist.angular.z = dth / dt # sensors lsb, rsb, lfb, rfb = self.robot.getDigitalSensors() # buttons btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons( ) # publish everything self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then, "base_footprint", "odom") self.scanPub.publish(scan) self.odomPub.publish(odom) button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button") sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper") for idx, b in enumerate( (btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)): if b == 1: button.value = b button.name = button_enum[idx] self.buttonPub.publish(button) for idx, b in enumerate((lsb, rsb, lfb, rfb)): if b == 1: sensor.value = b sensor.name = sensor_enum[idx] self.sensorPub.publish(sensor) # wait, then do it again r.sleep() # shut down self.robot.setBacklight(0) self.robot.setLED("Off") self.robot.setLDS("off") self.robot.setTestMode("off")
def talker(): rospy.init_node('infant_odometry', anonymous=True) #rospy.Subscriber("/cmd_vel", Twist, cmdvel_callback) rospy.Subscriber("/benz/raspi/encR", Int64, odom_callbackR) rospy.Subscriber("/benz/raspi/encL", Int64, odom_callbackL) #rospy.Subscriber("/imu", Imu, imu_callback) rospy.Subscriber("/imu/serial", String, imuserial_callback) pub = rospy.Publisher('/odom', Odometry, queue_size=10 ) odo=Odometry() odo.header.frame_id='odom' odo.child_frame_id='base_link' current_time=time.time() last_time=time.time() #======= Parameter Setting with Parameter Server======= # * If the parameter server didn't work, the following sequence would be passed. if rospy.has_param('infant_learning_odometry/right_wheel_p'): global D_RIGHT D_RIGHT=rospy.get_param('infant_learning_odometry/right_wheel_p') if rospy.has_param('infant_learning_odometry/left_wheel_p'): global D_LEFT PRIGHT=rospy.get_param('infant_learning_odometry/left_wheel_p') if rospy.has_param('infant_learning_odometry/Tread'): global TREAD TREAD=rospy.get_param('infant_learning_odometry/Tread') #print PRIGHT #print PLEFT #print TREAD #=========================================================== odo.pose.pose.position.x=0 odo.pose.pose.orientation.w=0 odo.twist.twist.linear.x=0 odo.twist.twist.angular.z=0 r = rospy.Rate(10) global dt global x global count global imuYaw global imuPitch x=array([0,0,toRadian(0.0)]); dt=0.1 t=0; while not rospy.is_shutdown(): count=count+1 current_time=time.time() dt=current_time-last_time odo.header.seq=count odo.header.stamp = rospy.Time.now() odo.pose.pose.position.x=x[0] odo.pose.pose.position.y=x[1] roll=imuRoll pitch=imuPitch yaw = x[2] q = quaternion_from_euler(roll,pitch,yaw) odo.pose.pose.orientation.x = q[0] odo.pose.pose.orientation.y = q[1] odo.pose.pose.orientation.z = q[2] odo.pose.pose.orientation.w = q[3] odo.twist.twist.linear.x=u[0] odo.twist.twist.angular.z=u[1] pub.publish(odo) # odom tf if bc is not None: bc.sendTransform((x[0],x[1],0.0),q,rospy.Time.now(),'base_link','odom') if count%2000==0: print "%8.2f" %x[0],"%8.2f" %x[1],"%8.2f" %toDegree(x[2]) #print (last_time-current_time) #print t #result.write('%f,%f,%f,%f,%f\n' %(x[0],x[1],x[2],u[0],u[1])) last_time=time.time() t+=dt r.sleep() rospy.spin()
from geometry_msgs.msg import Vector3 from geometry_msgs.msg import Point from sensor_msgs.msg import LaserScan from interactive_markers.interactive_marker_server import * from visualization_msgs.msg import * import numpy as np import random from numpy import ones, vstack from numpy.linalg import lstsq import math from nav_msgs.msg import Odometry from tf.transformations import euler_from_quaternion #goal is (4.5,9.0) odata = Odometry() ldata = LaserScan wallfollow = False def pol2cart(rt, tt): x = rt * np.cos(tt) y = rt * np.sin(tt) return (x, y) def distance(m, c, p, q): d = abs((m * p) + (-1 * q) + c) d = d / math.sqrt(m * m + 1) return d
def callback(self,data): rospy.loginfo_once("received data lat lon usbl") if(self.paramsReady()): rospy.loginfo_once("Valid survey params found: transform_pose starting") crs_wgs = proj.Proj(init='epsg:4326') # assuming you're using WGS84 geographic utm_zone = rospy.get_param("/utm_zone") crs_bng = proj.Proj(init=utm_zone) # use a locally appropriate projected CRS originLon = rospy.get_param("/origin_lon") originLat = rospy.get_param("/origin_lat") # then cast your geographic coordinate pair to the projected system xoff, yoff = proj.transform(crs_wgs, crs_bng, originLon, originLat) zoff = rospy.get_param("/origin_z",0) # input from NavSatFix lon = data.longitude; lat = data.latitude; x, y = proj.transform(crs_wgs, crs_bng, lon, lat) #x = x-xoff #y = y-yoff # small correction #x = x-(193543.42 +1371) #y = y-(2208649.25 -3967) x = x-193543.42 -1371 y = y-2208649.25 +3967 z = 0; static_transformStamped = geometry_msgs.msg.TransformStamped() static_transformStamped.header.stamp = rospy.Time.now() static_transformStamped.header.frame_id = "utm" static_transformStamped.child_frame_id = "map" static_transformStamped.transform.translation.x = xoff static_transformStamped.transform.translation.y = yoff static_transformStamped.transform.translation.z = 0 quat = tf.transformations.quaternion_from_euler(0,0,0) static_transformStamped.transform.rotation.x = quat[0] static_transformStamped.transform.rotation.y = quat[1] static_transformStamped.transform.rotation.z = quat[2] static_transformStamped.transform.rotation.w = quat[3] self.br.sendTransform(static_transformStamped) # self.br.sendTransform((xoff, yoff, 0), # tf.transformations.quaternion_from_euler(0, 0, 0), # data.header.stamp, # "map", # "utm") # output message transformed_msg = Odometry() transformed_msg.header.stamp = data.header.stamp; transformed_msg.child_frame_id = "base_link" if x== float('Inf') or y==float('Inf'): pass else: transformed_msg.pose.pose.orientation.w = 1.0 transformed_msg.pose.pose.position.x=x transformed_msg.pose.pose.position.y=y transformed_msg.pose.pose.position.z=z transformed_msg.header.frame_id="map" transformed_msg.pose.covariance = [100., 0., 0., 0., 0., 0., 0., 100., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1.] self.pub.publish(transformed_msg) if self.pressed: self.button_pub.publish(transformed_msg) self.pressed = False else: rospy.loginfo_once("Receiving odom message but survey params not ready. waiting...")
def odom_callback(self, odom_msg): self.slow_vel_count += 1 if (self.initialization): self.initialization = 0 w_0 = 1. / self.M * np.ones((5, 1)) self.gsf_obj = GSF(self.M, w_0) for i in range(self.M): # Establish x_0 if i == 0: x_0 = np.empty([5, 1]) x_0[0] = odom_msg.pose.pose.position.x + 0.1 x_0[1] = odom_msg.pose.pose.position.y orientation_quat = odom_msg.pose.pose.orientation orientation_euler = tf.transformations.euler_from_quaternion( [ orientation_quat.x, orientation_quat.y, orientation_quat.z, orientation_quat.w ]) x_0[2] = orientation_euler[2] x_0[3] = 0 x_0[4] = 0 if i == 1: x_0 = np.empty([5, 1]) x_0[0] = odom_msg.pose.pose.position.x - 0.1 x_0[1] = odom_msg.pose.pose.position.y orientation_quat = odom_msg.pose.pose.orientation orientation_euler = tf.transformations.euler_from_quaternion( [ orientation_quat.x, orientation_quat.y, orientation_quat.z, orientation_quat.w ]) x_0[2] = orientation_euler[2] x_0[3] = 0 x_0[4] = 0 if i == 2: x_0 = np.empty([5, 1]) x_0[0] = odom_msg.pose.pose.position.x x_0[1] = odom_msg.pose.pose.position.y + 0.1 orientation_quat = odom_msg.pose.pose.orientation orientation_euler = tf.transformations.euler_from_quaternion( [ orientation_quat.x, orientation_quat.y, orientation_quat.z, orientation_quat.w ]) x_0[2] = orientation_euler[2] x_0[3] = 0 x_0[4] = 0 if i == 3: x_0 = np.empty([5, 1]) x_0[0] = odom_msg.pose.pose.position.x x_0[1] = odom_msg.pose.pose.position.y - 0.1 orientation_quat = odom_msg.pose.pose.orientation orientation_euler = tf.transformations.euler_from_quaternion( [ orientation_quat.x, orientation_quat.y, orientation_quat.z, orientation_quat.w ]) x_0[2] = orientation_euler[2] x_0[3] = 0 x_0[4] = 0 if i == 4: x_0 = np.empty([5, 1]) x_0[0] = odom_msg.pose.pose.position.x x_0[1] = odom_msg.pose.pose.position.y self.x_prev = x_0[0] self.y_prev = x_0[1] orientation_quat = odom_msg.pose.pose.orientation orientation_euler = tf.transformations.euler_from_quaternion( [ orientation_quat.x, orientation_quat.y, orientation_quat.z, orientation_quat.w ]) x_0[2] = orientation_euler[2] x_0[3] = 0 x_0[4] = 0 # Establish P_0 P_0 = 0.1 * np.identity(5) # Initialize EKFs self.gsf_obj.gsf_fill_dict(i, x_0, P_0) self.time_prev = odom_msg.header.stamp self.slow_time_prev = self.time_prev time_curr = odom_msg.header.stamp d = time_curr - self.time_prev delta_t = d.to_sec() # Establish u_k from imu_msg imu_msg = rospy.wait_for_message("/L01/imu_raw", Imu) u = np.empty([3, 1]) u[0][0] = imu_msg.angular_velocity.z u[1][0] = imu_msg.linear_acceleration.x u[2][0] = imu_msg.linear_acceleration.y self.gsf_obj.gsf_predict(u, delta_t) # Establish y_k+1 from odom msg y = np.empty([3, 1]) y[0][0] = odom_msg.pose.pose.position.x y[1][0] = odom_msg.pose.pose.position.y orientation_quat = odom_msg.pose.pose.orientation orientation_euler = tf.transformations.euler_from_quaternion([ orientation_quat.x, orientation_quat.y, orientation_quat.z, orientation_quat.w ]) y[2][0] = orientation_euler[2] # Can possibly create fake velocities here? if (self.slow_vel_count % 100 == 0): # Find Current values slow_time_curr = odom_msg.header.stamp x_curr = y[0] y_curr = y[1] slow_time_delta = slow_time_curr - self.slow_time_prev delta_t_slow = slow_time_delta.to_sec() # Find velocitie estimates x_vel = (x_curr - self.x_prev) / delta_t_slow y_vel = (y_curr - self.y_prev) / delta_t_slow y_new = np.empty([5, 1]) y_new[0] = y[0] y_new[1] = y[1] y_new[2] = y[2] y_new[3] = x_vel y_new[4] = y_vel # Set previous values to current values self.x_prev = x_curr self.y_prev = y_curr self.slow_time_prev = slow_time_curr y = y_new # Given measurements, correct x_hat estimate self.gsf_obj.gsf_correct(y, delta_t) # Get and publish MMSE odom_output = Odometry() odom_output.header.stamp = rospy.Time.now() odom_output.header.frame_id = self.frame_id mu_k_plus_one_plus = self.gsf_obj.get_mu() self.x_store = np.append(self.x_store, mu_k_plus_one_plus, axis=1) rotation_quat = tf.transformations.quaternion_from_euler( 0, 0, mu_k_plus_one_plus[2]) odom_output.pose.pose = Pose( Point(mu_k_plus_one_plus[0], mu_k_plus_one_plus[1], 0), Quaternion(*rotation_quat)) odom_output.twist.twist = Twist( Vector3(mu_k_plus_one_plus[3], mu_k_plus_one_plus[4], 0), Vector3(0, 0, u[0])) self.filter_output_pub.publish(odom_output) Sigma_k_plus_one_plus = self.gsf_obj.get_sigma() self.P_store = np.append(self.P_store, Sigma_k_plus_one_plus.reshape(25, 1), axis=1) P_msg = Float64MultiArray() P_msg.layout.dim.append(MultiArrayDimension()) P_msg.layout.dim[0].size = 5 P_msg.layout.dim[0].stride = 25 P_msg.layout.dim.append(MultiArrayDimension()) P_msg.layout.dim[1].size = 5 P_msg.layout.dim[1].stride = 5 P_msg.data = Sigma_k_plus_one_plus.reshape(25) # rospy.loginfo("Publishing y") self.P_publish.publish(P_msg) self.time_prev = time_curr if (self.slow_vel_count == 22000): #print "Saving to ekf.mat" #scipy.io.savemat('ekf',{'x': self.x_store, 'P': self.P_store}) self.odom_sub.unregister() print "Save to csv" np.savetxt("/home/kyle/catkin_ws/src/ASEN_Project/bag/gsf_x.csv", self.x_store, delimiter=",") np.savetxt("/home/kyle/catkin_ws/src/ASEN_Project/bag/gsf_P.csv", self.P_store, delimiter=",") print "Save complete" rospy.signal_shtudown("Ended GSF")