def SetLocalLogger(self, allowLocalLogging, localFileName): self.logger.SetLocalLogger(self._to_cpp(Bool(allowLocalLogging)), self._to_cpp(String(localFileName)))
def Running(self): reset_msg = Bool() reset_msg.data = True self.resetPub.publish(reset_msg) rospy.sleep(1) self.odom.header.frame_id = "odom" self.odom.child_frame_id = "base_footprint" self.odom.pose.pose.position.z = 0 while not rospy.is_shutdown(): # Calculate robot postion deltaLeft = self.left_count - self.last_left deltaRight = self.right_count - self.last_right delta_time = float(str(rospy.Time.now() - self.time))/1000000000 self.last_left = self.left_count self.last_right = self.right_count if math.fabs(deltaLeft) > 10000 or math.fabs(deltaRight) > 10000: continue if self.left_count == 0 and self.right_count ==0: if math.fabs(deltaLeft) > 0 or math.fabs(deltaRight) > 0: continue deltaDistance = (deltaLeft + deltaRight) * self.distance_per_count / 2 deltaAngle = (deltaRight - deltaLeft) * self.distance_per_count / self.track_width self.x += deltaDistance * math.cos(self.pose.theta) self.y += deltaDistance * math.sin(self.pose.theta) self.theta += deltaAngle self.v = (self.left_speed + self.right_speed) / 2 self.w = (self.right_speed - self.left_speed) / self.track_width # Publish odometry quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(self.theta / 2.0) quaternion.w = cos(self.theta / 2.0) ros_now = rospy.Time.now() self._OdometryTransformBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), ros_now, "base_footprint", "odom" ) odometry = Odometry() odometry.header.frame_id = "odom" odometry.header.stamp = ros_now odometry.pose.pose.position.x = self.x odometry.pose.pose.position.y = self.y odometry.pose.pose.position.z = 0 odometry.pose.pose.orientation = quaternion odometry.child_frame_id = "base_link" odometry.twist.twist.linear.x = self.v odometry.twist.twist.linear.y = 0 odometry.twist.twist.angular.z = self.w self._OdometryPublisher.publish(odometry) # Set the frame and position for the odometry odometry = odo odometry.header.frame_id = "odom" odometry.header.stamp = ros_now odometry.child_frame_id = "zed" odometry.pose.pose.position.z = 0 self.rate.sleep()
def main(args): global wait_measurement_sensor_flag global wait_measurement_pos_x_flag global wait_measurement_pos_y_flag global current_position global current_state global polygon_voronoi_publisher global list_current_position, poses global list_current_velocity global local_vel_pub global local_pos_pub global init_coverage_start global mission_state global prev_position global mission_state_pub, RVO_pub_list, rviz_visualization_start global uav_marker,uav_marker_publisher,uav_goal_marker,uav_goal_marker_publisher,br,uav_idx,uav_ID #thread.join() par = lp.Parameter re = les.RecursiveEstimation true_function = lp.TrueFunction(par) est_function = re.DensityFunction ne = les.NoEstimation par.CurrentIteration = 1 #print "Program Started" dir_path = os.path.dirname(os.path.realpath(__file__)) print dir_path with open(dir_path+'/config.json','r') as f: config = json.load(f) uav_ID = "1" x_val = 0 y_val = 0 z_val = 0 for i in range(len(args)): if(args[i] == "-uav"): uav_ID = args[i+1] elif(args[i] == "-x"): x_val = float(args[i+1]) elif(args[i] == "-y"): y_val = float(args[i+1]) elif(args[i] == "-z"): z_val = float(args[i+1]) elif(args[i] == "-rwd"): rwd_val = args[i+1] init_pos = config["UAV"][uav_ID]["INIT_POS"] uav_color = config["UAV"][uav_ID]["COLOR"] print "init pos: ",init_pos rospy.init_node("control_uav_"+uav_ID) uav_idx = int(uav_ID)-1 br = tf.TransformBroadcaster() publish_uav_position_rviz(br,init_pos[0],init_pos[1],init_pos[2],uav_ID) pathname=os.path.abspath(rwd_val+'/src/coverage_control_uavs/control_uav/scripts/') print pathname listfolder = [] for foldername in os.listdir(pathname+'/img_res'): listfolder.append(int(foldername)) #if fname.endswith('.mp4'): # listvideo.append(fname) if(len(listfolder) == 0): curr_folder = 0 else: curr_folder = np.max(listfolder) print 'current folder:',curr_folder folderstring = pathname+'/img_res/%04d/'%curr_folder if(uav_ID == '1'): os.system("cp -i "+dir_path+'/config.json '+folderstring) print "cp -i "+dir_path+'/config.json '+folderstring try: rospy.wait_for_service("/uav"+uav_ID+"/mavros/cmd/arming") rospy.wait_for_service("/uav"+uav_ID+"/mavros/set_mode") except rospy.ROSException: fail("failed to connect to service") state_sub=rospy.Subscriber("/uav"+uav_ID+"/mavros/state", State,queue_size=10,callback=state_cb) #velocity_sub=rospy.Subscriber("/uav"+uav_ID+"/mavros/local_position/velocity",) #list_UAV_ID,num_agent = get_number_of_agent() #num_agent = len(config["UAV"]) list_UAV_ID = [] all_init_pos = [] for i in range(par.NumberOfAgents): list_UAV_ID.append("uav"+repr(i+1)) all_init_pos.append(config["UAV"][repr(i+1)]["INIT_POS"]) print "NUM OF AGENT = ",par.NumberOfAgents print "LIST UAV = ",list_UAV_ID do_coverage_flag = Bool() do_coverage_flag.data = False reach_goal_flag = Bool() reach_goal_flag.data = True var_float = Float32() var_float.data = 0.0 for i in range(par.NumberOfAgents): list_current_position.append(PoseStamped()) list_current_velocity.append(TwistStamped()) list_reach_goal.append(reach_goal_flag) list_do_coverage.append(do_coverage_flag) #list_measurement_sensor.append(var_float) list_measurement_sensor.append(0.0) wait_measurement_sensor_flag.append(0.0) #list_measurement_pos_x.append(var_float) list_measurement_pos_x.append(0.0) wait_measurement_pos_x_flag.append(0.0) #list_measurement_pos_y.append(var_float) list_measurement_pos_y.append(0.0) wait_measurement_pos_y_flag.append(0.0) V_max = [] for i in range(par.NumberOfAgents): velocity_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb, callback_args=i) position_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/pose",PoseStamped,queue_size = 10,callback=position_cb,callback_args=(i,uav_idx,all_init_pos)) reach_goal_sub = rospy.Subscriber("/uav"+repr(i+1)+"/reach_goal",Bool,queue_size=10,callback=reach_goal_cb,callback_args=i) do_coverage_sub = rospy.Subscriber("/uav"+repr(i+1)+"/do_coverage",Bool,queue_size=10,callback=do_coverage_cb,callback_args=i) measurement_sensor_sub = rospy.Subscriber("/uav"+repr(i+1)+"/measurement_sensor",Float32MultiArray,queue_size=10,callback=measurement_sensor_cb,callback_args=i) measurement_pos_x_sub = rospy.Subscriber("/uav"+repr(i+1)+"/measurement_pos_x",Float32MultiArray,queue_size=10,callback=measurement_pos_x_cb,callback_args=i) measurement_pos_y_sub = rospy.Subscriber("/uav"+repr(i+1)+"/measurement_pos_y",Float32MultiArray,queue_size=10,callback=measurement_pos_y_cb,callback_args=i) V_max.append(par.V_MAX) RVO_pub_list = [] for i in range(len(par.ws_model['circular_obstacles'])): RVO_pub_list.append(rospy.Publisher("/uav"+uav_ID+"/RVO"+repr(i),Float32MultiArray,queue_size=10)) scan_lidar_sub = rospy.Subscriber("/scan",LaserScan,queue_size=10,callback=scan_lidar_cb,callback_args=uav_ID) #velocity_sub = rospy.Subscriber("/uav1/mavros/setpoint_raw/global",GlobalPositionTarget,queue_size = 10,callback=velocity_cb) #velocity_sub = rospy.Subscriber("/uav1/mavros/setpoint_raw/global",PositionTarget,queue_size = 10,callback=velocity_cb) #velocity_sub = rospy.Subscriber("/uav1/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb) #position_sub = rospy.Subscriber("/uav1/mavros/local_position/pose",PoseStamped,queue_size = 10,callback=position_cb) global local_pos_pub, poses local_pos_pub=rospy.Publisher("/uav"+uav_ID+"/mavros/setpoint_position/local",PoseStamped,queue_size=10) local_vel_pub=rospy.Publisher("/uav"+uav_ID+"/mavros/setpoint_velocity/cmd_vel",TwistStamped,queue_size=10) goal_pos_pub=rospy.Publisher("/uav"+uav_ID+"/goal_position",PoseStamped,queue_size=10) sub_goal_pos_pub=rospy.Publisher("/uav"+uav_ID+"/sub_goal_position",PoseStamped,queue_size=10) local_reach_goal_flag_pub=rospy.Publisher("/uav"+uav_ID+"/reach_goal",Bool,queue_size=10) local_do_coverage_flag_pub=rospy.Publisher("/uav"+uav_ID+"/do_coverage",Bool,queue_size=10) mission_state_pub=rospy.Publisher("/uav"+uav_ID+"/mission_state",Int32,queue_size=10) #local_measurement_sensor_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_sensor",Float32,queue_size=10) #local_measurement_pos_x_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_pos_x",Float32,queue_size=10) #local_measurement_pos_y_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_pos_y",Float32,queue_size=10) local_measurement_sensor_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_sensor",Float32MultiArray,queue_size=10) local_measurement_pos_x_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_pos_x",Float32MultiArray,queue_size=10) local_measurement_pos_y_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_pos_y",Float32MultiArray,queue_size=10) polygon_voronoi_publisher = rospy.Publisher("/uav"+uav_ID+"/polygon_voronoi",Polygon,queue_size=10) voronoi_grid_publisher = rospy.Publisher("/uav"+uav_ID+"/voronoi_grid",Int8MultiArray,queue_size=10) current_iteration_publisher = rospy.Publisher("/current_iteration",Int32,queue_size=10) voronoi_grid = Int8MultiArray() voronoi_grid.layout.dim.append(MultiArrayDimension()) voronoi_grid.layout.dim.append(MultiArrayDimension()) voronoi_grid.layout.dim[0].label = "height" voronoi_grid.layout.dim[1].label = "width" voronoi_grid.layout.dim[0].size = par.NumberOfPoints voronoi_grid.layout.dim[1].size = par.NumberOfPoints voronoi_grid.layout.dim[0].stride = par.NumberOfPoints*par.NumberOfPoints voronoi_grid.layout.dim[1].stride = par.NumberOfPoints voronoi_grid.layout.data_offset = 0 voronoi_grid.data = np.zeros((par.NumberOfPoints,par.NumberOfPoints)).reshape((1,par.NumberOfPoints*par.NumberOfPoints)).tolist()[0] polygon_msg = Polygon() meas_sensor = Float32MultiArray() meas_sensor.layout.dim.append(MultiArrayDimension()) meas_sensor.layout.dim[0].label = "length" meas_sensor.layout.dim[0].size = 2 meas_sensor.layout.dim[0].stride = 2 meas_sensor.layout.data_offset = 0 meas_sensor.data = [0.0,0.0] meas_pos_x = Float32MultiArray() meas_pos_x.layout.dim.append(MultiArrayDimension()) meas_pos_x.layout.dim[0].label = "length" meas_pos_x.layout.dim[0].size = 2 meas_pos_x.layout.dim[0].stride = 2 meas_pos_x.layout.data_offset = 0 meas_pos_x.data = [0.0,0.0] meas_pos_y = Float32MultiArray() meas_pos_y.layout.dim.append(MultiArrayDimension()) meas_pos_y.layout.dim[0].label = "length" meas_pos_y.layout.dim[0].size = 2 meas_pos_y.layout.dim[0].stride = 2 meas_pos_y.layout.data_offset = 0 meas_pos_y.data = [0.0,0.0] est_function_pc = Float32MultiArray() est_function_pc.layout.dim.append(MultiArrayDimension()) est_function_pc.layout.dim.append(MultiArrayDimension()) est_function_pc.layout.dim[0].label = "height" est_function_pc.layout.dim[1].label = "width" est_function_pc.layout.dim[0].size = est_function.shape[0] est_function_pc.layout.dim[1].size = est_function.shape[1] est_function_pc.layout.dim[0].stride = est_function.shape[0]*est_function.shape[1] est_function_pc.layout.dim[1].stride = est_function.shape[1] est_function_pc.layout.data_offset = 0 est_function_pc.data = est_function.reshape((1,est_function.shape[0]*est_function.shape[1])).tolist()[0] est_sensory_function_pub=rospy.Publisher("/uav"+uav_ID+"/est_sensory_function",Float32MultiArray,queue_size=10) arming_client = rospy.ServiceProxy("/uav"+uav_ID+"/mavros/cmd/arming",CommandBool) set_mode_client = rospy.ServiceProxy("/uav"+uav_ID+"/mavros/set_mode",SetMode) topic = 'uav_marker_'+uav_ID uav_marker_publisher = rospy.Publisher(topic, Marker,queue_size=10) uav_marker = Marker() uav_marker.header.frame_id = "world" uav_marker.type = uav_marker.SPHERE uav_marker.action = uav_marker.ADD uav_marker.scale.x = par.ws_model['robot_radius']*3 uav_marker.scale.y = par.ws_model['robot_radius']*3 uav_marker.scale.z = 0.005*par.RealScale uav_marker.color.r = float(uav_color[0])/255 uav_marker.color.g = float(uav_color[1])/255 uav_marker.color.b = float(uav_color[2])/255 uav_marker.color.a = float(uav_color[3])/255 uav_marker.pose.orientation.w = 1.0 uav_marker.pose.position.x = init_pos[0] uav_marker.pose.position.y = init_pos[1] uav_marker.pose.position.z = init_pos[2] uav_marker_publisher.publish(uav_marker) poses = PoseStamped() #poses.pose = Pose() #poses.pose.position = Point() poses.pose.position.x = x_val poses.pose.position.y = y_val poses.pose.position.z = z_val uav_color = [255,255,255,255] topic = 'uav_goal_marker_'+uav_ID uav_goal_marker_publisher = rospy.Publisher(topic, Marker,queue_size=10) uav_goal_marker = Marker() uav_goal_marker.header.frame_id = "world" uav_goal_marker.type = uav_goal_marker.CUBE uav_goal_marker.action = uav_goal_marker.ADD uav_goal_marker.scale.x = par.ws_model['robot_radius']*2 uav_goal_marker.scale.y = par.ws_model['robot_radius']*2 uav_goal_marker.scale.z = 0.005*par.RealScale uav_goal_marker.color.r = float(uav_color[0])/255 uav_goal_marker.color.g = float(uav_color[1])/255 uav_goal_marker.color.b = float(uav_color[2])/255 uav_goal_marker.color.a = float(uav_color[3])/255 uav_goal_marker.pose.orientation.w = 1.0 uav_goal_marker.pose.position.x = 0#init_pos[0] uav_goal_marker.pose.position.y = 0#init_pos[1] uav_goal_marker.pose.position.z = 0#init_pos[2] uav_goal_marker_publisher.publish(uav_goal_marker) uav_goal_marker=update_uav_marker(uav_goal_marker,(poses.pose.position.x,poses.pose.position.y,poses.pose.position.z,1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) r=rospy.Rate(10) vController = VelocityController() #print(dir(current_state.connected)) print "TRY TO CONNECT" while ((not rospy.is_shutdown()) and (not current_state.connected)): #rospy.spinOnce() r.sleep() print "CONNECTED" #print(current_state.connected.__class__) rospy.loginfo("CURRENT STATE CONNECTED") init_position_coverage = (x_val,y_val,z_val) print poses.pose.position.x,poses.pose.position.y,poses.pose.position.z i = 100 while((not rospy.is_shutdown()) and (i>0)): local_pos_pub.publish(poses) i = i-1 rospy.loginfo("POSITION PUBLISHED") publish_mission_state(mission_state) thread1 = Thread(target = pose_pub_function) thread1.start() thread2 = Thread(target = velocity_pub_function,args=(par,int(uav_ID)-1,vController)) thread2.start() thread3 = Thread(target = error_pub_function,args=(par,int(uav_ID)-1)) thread3.start() last_request = rospy.Time.now() last_request_neighbor = rospy.Time.now() last_request_rviz = rospy.Time.now() last_request_debug = rospy.Time.now() last_iteration_timeout = rospy.Time.now() rviz_visualization_start = False coverage_start = False coverage_end = False averageErrorDensityFunction = [] timePerIteration = [] timeCoverage = [] while(not rospy.is_shutdown()): #offb_set_mode = set_mode_client(0,'OFFBOARD') if(rviz_visualization_start and (rospy.Time.now()-last_request_rviz > rospy.Duration(0.2))): #publish_uav_position_rviz(br,list_current_position[uav_idx].pose.position.x,list_current_position[uav_idx].pose.position.y,list_current_position[uav_idx].pose.position.z,uav_ID) #uav_marker=update_uav_marker(uav_marker,(list_current_position[uav_idx].pose.position.x,list_current_position[uav_idx].pose.position.y,list_current_position[uav_idx].pose.position.z,1.0)) #uav_marker_publisher.publish(uav_marker) #uav_goal_marker=update_uav_marker(uav_goal_marker,(poses.pose.position.x,poses.pose.position.y,poses.pose.position.z,1.0)) #uav_goal_marker_publisher.publish(uav_goal_marker) last_request_rviz = rospy.Time.now() #print "list_reach_goal",list_reach_goal if(rviz_visualization_start and (rospy.Time.now()-last_request_debug > rospy.Duration(1.0))): last_request_debug = rospy.Time.now() #print "+++ DEBUG +++" #print list_do_coverage #print "+++++++++++++" if(rospy.Time.now()-last_request_neighbor > rospy.Duration(10.0)): #for i in range(num_agent): # print list_current_position[i].pose.position.x,list_current_position[i].pose.position.y last_request_neighbor = rospy.Time.now() if((not rviz_visualization_start) and (current_state.mode != 'OFFBOARD') and (rospy.Time.now()-last_request > rospy.Duration(1.0))): offb_set_mode = set_mode_client(0,'OFFBOARD') if(offb_set_mode.mode_sent): rospy.loginfo("OFFBOARD ENABLED") last_request = rospy.Time.now() else: if((not current_state.armed) and (rospy.Time.now()-last_request > rospy.Duration(1.0))): arm_cmd = arming_client(True) if(arm_cmd.success): rospy.loginfo("VEHICLE ARMED") rviz_visualization_start = True #coverage_start = True reach_goal_flag = Bool() reach_goal_flag.data = True last_request_rviz = rospy.Time.now() last_request=rospy.Time.now() if(par.CurrentIteration == 1) and not coverage_start: #print "current position", current_position #print "init pos coverage", init_position_coverage #if(distance3D(init_position_coverage,current_position)<3): if(rospy.Time.now()-last_iteration_timeout > rospy.Duration(5.0)): last_iteration_timeout = rospy.Time.now() target = Pose() targetStamped = PoseStamped() target.position.x = poses.pose.position.x target.position.y = poses.pose.position.y target.position.z = poses.pose.position.z diagram4 = GridWithWeights(par.NumberOfPoints,par.NumberOfPoints) diagram4.walls = [] for i in range(par.NumberOfPoints): # berikan wall pada algoritma pathfinding A* for j in range(par.NumberOfPoints): if (par.ObstacleRegion[j,i]==0): diagram4.walls.append((i,j)) for i in range(par.NumberOfPoints): for j in range(par.NumberOfPoints): if(par.ObstacleRegionWithClearence[j][i] == 0): diagram4.weights.update({(i,j): par.UniversalCost}) goal_pos = (target.position.x/par.RealScale,target.position.y/par.RealScale) start_pos = (cur_pose.pose.position.x/par.RealScale,cur_pose.pose.position.y/par.RealScale) pathx,pathy=Astar_version1(par,start_pos,goal_pos,diagram4) target.position.x = pathx[0]*par.RealScale target.position.y = pathy[0]*par.RealScale vController.setHeadingTarget(deg2rad(45.0)) vController.setTarget(target) vController.setPIDGainX(1,0.0,0.0) vController.setPIDGainY(1,0.0,0.0) vController.setPIDGainZ(1,0,0) vController.setPIDGainPHI(1,0.0,0.0) vController.setPIDGainTHETA(1,0.0,0.0) vController.setPIDGainPSI(1,0.0,0.0) init_coverage_start = True coverage_start = True V_msg = TwistStamped() V_msg.header.stamp = rospy.Time.now() V_msg.twist.linear.x =0 # V_msg.twist.linear.y =0 # V_msg.twist.linear.z = 0 V_msg.twist.angular.x = 0.0 V_msg.twist.angular.y = 0.0 V_msg.twist.angular.z = 0.0 #w local_vel_pub.publish(V_msg) mission_state = AGENT_ARRIVE_AT_GOAL publish_mission_state(mission_state) if(coverage_start): # and all(item.data == True for item in list_reach_goal)): # coverage #gaussian regression print "time:" , datetime.datetime.now().strftime("%H:%M:%S") print "do coverage iter-",par.CurrentIteration do_coverage_flag = Bool() do_coverage_flag.data = True x = (list_current_position[uav_idx].pose.position.x/par.RealScale,list_current_position[uav_idx].pose.position.y/par.RealScale) y = com.transmitNewMeasurementOneAgent(par,x) #measurement.data = [par.CurrentIteration,x[0],x[1],y] for i in range(par.NumberOfAgents): meas_sensor.data = [par.CurrentIteration,y] meas_pos_x.data = [par.CurrentIteration,x[0]] meas_pos_y.data = [par.CurrentIteration,x[1]] local_measurement_sensor_pub.publish(meas_sensor) local_measurement_pos_x_pub.publish(meas_pos_x) local_measurement_pos_y_pub.publish(meas_pos_y) if(par.UsingStrategy == 2): while not (wait_measurement_sensor_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2): pass while not (wait_measurement_pos_x_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2): pass while not (wait_measurement_pos_y_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2): pass for i in range(par.NumberOfAgents): if(wait_measurement_sensor_flag[i] == par.CurrentIteration) and (wait_measurement_pos_x_flag[i] == par.CurrentIteration) and (wait_measurement_pos_y_flag[i] == par.CurrentIteration): re.statusFinishedAgent[i] = 0 if(par.UsingStrategy == 1): while not all(item == par.CurrentIteration for item in wait_measurement_sensor_flag): pass while not all(item == par.CurrentIteration for item in wait_measurement_pos_x_flag): pass while not all(item == par.CurrentIteration for item in wait_measurement_pos_y_flag): pass #print "wait_meas_sensor",wait_measurement_sensor_flag #print "wait_meas_pos_x",wait_measurement_pos_x_flag #print "wait_meas_pos_y",wait_measurement_pos_y_flag #for i in range(par.NumberOfAgents): # wait_measurement_sensor_flag[i] = True # wait_measurement_pos_x_flag[i] = True # wait_measurement_pos_y_flag[i] = True print "list_measurement:",list_measurement_sensor,list_measurement_pos_x,list_measurement_pos_y # wait all agent measure iteration_msg = Int32() iteration_msg.data = par.CurrentIteration if(uav_ID == '1'): current_iteration_publisher.publish(iteration_msg) if(uav_ID == '1'): if(par.CurrentIteration>1): timePerIteration.append(time.time()-timePerIterationStart) f= open(folderstring+"/timeiter.txt","a+") f.write("%f\r\n" % (timePerIteration[par.CurrentIteration-2])) f.close() timePerIterationStart = time.time() timeCoverageStart = time.time() re = com.joinNewMeasurements(par,re,list_measurement_sensor,list_measurement_pos_x,list_measurement_pos_y) timeCoverageStart = time.time() re = com.computeCentralizedEstimation(re,par) re = com.computeCoverage(re,par,re.APosterioriVariance,par.ws_model) if(uav_ID == '1'): timeCoverage.append(time.time()-timeCoverageStart) selisih = re.DensityFunction - true_function averageErrorDensityFunction.append(np.sum(np.sum(np.multiply(selisih,selisih)))/(par.NumberOfPoints*par.NumberOfPoints)) re.CostFunction.append(com.computeCost(par,re.Node,re.DensityFunction,par.CostFlag)) f= open(folderstring+"/maxvar.txt","a+") f.write("%f\r\n" % (re.maxAPosterioriVariance[par.CurrentIteration-1])) f.close() f= open(folderstring+"/minvar.txt","a+") f.write("%f\r\n" % (re.minAPosterioriVariance[par.CurrentIteration-1])) f.close() f= open(folderstring+"/avgvar.txt","a+") f.write("%f\r\n" % (re.averageAPosterioriVariance[par.CurrentIteration-1])) f.close() f= open(folderstring+"/avgdens.txt","a+") f.write("%f\r\n" % (averageErrorDensityFunction[par.CurrentIteration-1])) f.close() f= open(folderstring+"/timecoverage.txt","a+") f.write("%f\r\n" % (timeCoverage[par.CurrentIteration-1])) f.close() f= open(folderstring+"/cost.txt","a+") f.write("%f\r\n" % (re.CostFunction[par.CurrentIteration-1])) f.close() f= open(folderstring+"/energy.txt","a+") f.write("%f\r\n" % (re.averageEnergy[par.CurrentIteration-1])) f.close() est_function = re.DensityFunction est_function_pc.data = est_function.reshape((1,est_function.shape[0]*est_function.shape[1])).tolist()[0] if(uav_ID == '1'): est_sensory_function_pub.publish(est_function_pc) prev_position = (poses.pose.position.x, poses.pose.position.y, poses.pose.position.z) poses.pose.position.x = re.Node[uav_idx].pos[0]*par.RealScale poses.pose.position.y = re.Node[uav_idx].pos[1]*par.RealScale #print poses.pose.position.x,poses.pose.position.y poses.pose.position.z = par.StaticAltitude goal_pos_pub.publish(poses) mission_state = AGENT_GOES_TO_GOAL #AGENT_HOVER_YAWING publish_mission_state(mission_state) # if(control_type == PID_CONTROL): target = Pose() target.position.x = poses.pose.position.x target.position.y = poses.pose.position.y target.position.z = poses.pose.position.z vController.setHeadingTarget(deg2rad(45.0)) vController.setTarget(target) vController.setPIDGainX(1,0.0,0.0) vController.setPIDGainY(1,0.0,0.0) vController.setPIDGainZ(1,0,0) vController.setPIDGainPHI(1,0.0,0.0) vController.setPIDGainTHETA(1,0.0,0.0) vController.setPIDGainPSI(1,0.0,0.0) goal_pos = (target.position.x/par.RealScale,target.position.y/par.RealScale) start_pos = (cur_pose.pose.position.x/par.RealScale,cur_pose.pose.position.y/par.RealScale) pathx,pathy=Astar_version1(par,start_pos,goal_pos,diagram4) target.position.x = pathx[0]*par.RealScale target.position.y = pathy[0]*par.RealScale vController.setHeadingTarget(deg2rad(45.0)) vController.setTarget(target) targetStamped.pose = target sub_goal_pos_pub.publish(targetStamped) print "Max Variance [iter:"+repr(par.CurrentIteration)+"] : ",np.max(np.max(re.APosterioriVariance)) publish_polygon_voronoi(re.Node[uav_idx].pvicini,par.RealScale) voronoi_grid.data = re.Node[uav_idx].VoronoiRegion.reshape((1,par.NumberOfPoints*par.NumberOfPoints)).tolist()[0] voronoi_grid_publisher.publish(voronoi_grid) #print "Input Locations: ",re.InputLocations #reach_goal_flag.data = False #coverage_end = True coverage_start = False par.CurrentIteration = par.CurrentIteration +1 if init_coverage_start: if(distance2D(cur_pose.pose,target) < 3): if(len(pathx) > 1): del pathx[0] del pathy[0] target.position.x = pathx[0]*par.RealScale target.position.y = pathy[0]*par.RealScale print "change subtarget" vController.setTarget(target) targetStamped.pose = target sub_goal_pos_pub.publish(targetStamped) if (all(item.data == True for item in list_do_coverage)): #print "all done coverage" #print poses.pose.position.x,poses.pose.position.y #print list_current_position[uav_idx].pose.position.x,list_current_position[uav_idx].pose.position.y reach_goal_flag = Bool() reach_goal_flag.data = False pose_x_diff = (poses.pose.position.x-list_current_position[uav_idx].pose.position.x)**2 pose_y_diff = (poses.pose.position.y-list_current_position[uav_idx].pose.position.y)**2 x = (list_current_position[uav_idx].pose.position.x,list_current_position[uav_idx].pose.position.y,list_current_position[uav_idx].pose.position.z) goal = (poses.pose.position.x,poses.pose.position.y,poses.pose.position.z) timeout_flag = rospy.Time.now()-last_iteration_timeout > rospy.Duration(5.0) if((do_coverage_flag.data == True) and (reach_goal_flag.data == False) and (timeout_flag or ((par.UsingStrategy == 2) and (wait_measurement_sensor_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2)))) : #if((do_coverage_flag.data == True) and (reach_goal_flag.data == False) and ('''(distance3D(goal,x) < 2)'''timeout_flag or ((par.UsingStrategy == 2) and (wait_measurement_sensor_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2)))) : last_iteration_timeout = rospy.Time.now() mission_state = AGENT_ARRIVE_AT_GOAL publish_mission_state(mission_state) print "time:" , datetime.datetime.now().strftime("%H:%M:%S") print "agent "+uav_ID+" reach the goal" reach_goal_flag = Bool() reach_goal_flag.data = True # Node pos is current position for i in range(par.NumberOfAgents): re.Node[i].pos[0]=float(list_current_position[i].pose.position.x)/par.RealScale re.Node[i].pos[1]=float(list_current_position[i].pose.position.y)/par.RealScale #coverage_end = False do_coverage_flag = Bool() do_coverage_flag.data = False coverage_start = True for i in range(par.NumberOfAgents): local_reach_goal_flag_pub.publish(reach_goal_flag) while(not all(item.data == True for item in list_reach_goal)): #local_reach_goal_flag_pub.publish(reach_goal_flag) pass print "time:" , datetime.datetime.now().strftime("%H:%M:%S") print "list_reach_goal",list_reach_goal # publish everything local_do_coverage_flag_pub.publish(do_coverage_flag) local_reach_goal_flag_pub.publish(reach_goal_flag) #local_pos_pub.publish(poses) r.sleep() try: rospy.spin() except KeyboardInterrupt: print("Shutting down") cv2.destroyAllWindows()
from mobrob_util.msg import ME439WaypointXY, ME439SensorsProcessed from geometry_msgs.msg import Pose2D from std_msgs.msg import Bool # Waypoints to hit: a "numpy.array" of [x,y] coordinates. # Example: Square # waypoints = np.array([[0.5, 0.], [0.5, 0.5], [0., 0.5], [0., 0.]]) goal = np.array([0, 2]) ################################################################## # Run the Publisher ################################################################## # initialize the current "segment" to be the first one (index 0) # (you # could skip segments if you wanted to) waypoint_number = 0 # for waypoint seeking. path_complete = Bool() path_complete.data = False states = {'Standby': 0, 'Push': 1, 'Scan': 2, 'Reposition': 3} x = 0 y = 0 theta = 0 # TODO: Transition from Standby to a new goal state via subscribing # Publish desired waypoints at the appropriate time. def talker(): global waypoints, waypoint_number, path_complete, pub_waypoint global pub_path_complete, pub_find_object, states, state # Launch this node with the name "set_waypoints"
# let's define a function we will use later def shutdownFunction(): print "we are done here." # when the python interpreter runs this script # this is the section that gets run as main if __name__ == '__main__': # let the master know about our name rospy.init_node("riss") # define a duration of 1 second oneSecond = rospy.Duration(1, 0) # define and fill out some messages boolMsg = Bool() boolMsg.data = 0 uint16Msg = UInt16() uint16Msg.data = 10 uint16Pub = rospy.Publisher('unsigned_integer_topic', UInt16, queue_size=5) boolPub = rospy.Publisher('bool_topic', Bool, queue_size=5) # wait for Ctrl-C and loop until then while not rospy.is_shutdown(): # print a message for feedback print "looping..." # publish messages uint16Pub.publish(uint16Msg)
# Reset controllers, pause the sim, reset the world, # unpause the sim and then release the controllers winner_pub.publish(win_msg) reset_msg.data = True reset_pub.publish(reset_msg) gazebo_reset() reset_model(robot_1, robot_1_initial_pose) reset_model(robot_2, robot_2_initial_pose) reset_controllers() rospy.sleep(rospy.Duration(0.1)) reset_msg.data = False reset_pub.publish(reset_msg) start_time = rospy.Time.now() reset_pub = rospy.Publisher("/reset", Bool, queue_size=1) reset_msg = Bool() # Make sure all controllers are in the reset state reset_msg.data = True reset_pub.publish(reset_msg) rospy.sleep(rospy.Duration(0.5)) robot_1_initial_pose = robot_pos(robot_1, "world").pose robot_2_initial_pose = robot_pos(robot_2, "world").pose sleeper = rospy.Rate(20) robot_1_wins = 0 robot_2_wins = 0 draws = 0 it = 0 win_msg = String()
def action_start(): """ implements start end point """ if not check_action(request, START.url, START.methods): return th_error() log_das(LogError.INFO, "%s hit with %s" % (START.url, request.get_json(silent=True))) try: j = request.get_json(silent=True) params = TestAction(**j) except Exception as e: log_das(LogError.RUNTIME_ERROR, '%s got a malformed test action POST: %s' % (START.url, e)) return th_error() ## check to see if there's already an assigned goal, abort if so. global client if client.gh: log_das(LogError.RUNTIME_ERROR, "%s hit with an already active goal" % START.url) return th_error() global deadline global pub_user_notify global desired_volts global desired_bump global pub_setvoltage global pub_setcharging if config.enable_adaptation == AdaptationLevels.CP2_Adaptation: cw_log = open("/test/calibration_watcher.log", "w") cw_child = subprocess.Popen([BINDIR + "/calibration_watcher"], stdout=cw_log, stderr=cw_log, cwd="/home/vagrant/") if in_cp2() and (not bump_sensor(desired_bump)): log_das(LogError.RUNTIME_ERROR, "Fatal: could not set inital sensor pose in %s" % START.url) return th_error() try: pub_setcharging.publish(Bool(False)) pub_setvoltage.publish(Int32(desired_volts)) except Exception as e: log_das( LogError.RUNTIME_ERROR, '%s got an error trying to publish to set_voltage and set_charging: %s' % (START.url, e)) return th_error() log_das(LogError.INFO, "starting challenge problem") try: with open(instruct('.ig')) as igfile: igcode = igfile.read() goal = ig_action_msgs.msg.InstructionGraphGoal(order=igcode) client.send_goal(goal=goal, done_cb=done_cb, active_cb=active_cb) # update the deadline to be now + the amount of time for the path # given in the json file with open(instruct('.json')) as config_file: data = json.load(config_file) deadline = int(data['time']) + rospy.Time.now().secs pub_user_notify.publish( UserNotification(new_deadline=str(deadline), user_notification="initial deadline")) except Exception as e: log_das(LogError.RUNTIME_ERROR, "could not send the goal in %s: %s " % (START.url, e)) return th_error() res = action_result({}) log_das(LogError.INFO, "%s returning %s" % (START.url, resp2str(res))) return res
#!/home/mhyde/vEnvs/rosPy/bin/python import rospy from std_msgs.msg import Bool def pubReq(): while True: usrInput = raw_input('Send True? :: [Y/n] :: ') if usrInput.upper() == 'Y': msg.data = True pub.publish(msg) rospy.loginfo(msg) elif usrInput.lower() == 'end': break else: msg.data = False pub.publish(msg) rospy.loginfo(msg) msg = Bool() pub = rospy.Publisher('arduinoState', Bool, queue_size=10) rospy.init_node('dummyPub', anonymous=True) pubReq()
def send_exe_camera(self, BOOL): exe_msg = Bool() exe_msg.data = BOOL print("Publishing") self.pub_camera.publish(exe_msg)
#!/usr/bin/env python import sys import rospy import os import sys from gantry_control.msg import * from return_control.srv import * from std_msgs.msg import Bool from std_srvs.srv import Empty, Trigger from record_ros.srv import String_cmd from data_manager.srv import * import getFiles as files trueMsg = Bool() trueMsg.data = True falseMsg = Bool() falseMsg.data = False camState = False runComplete = False SAVE_DATA = rospy.get_param('GLOBAL_SAVE_DATA') DEBUG = False def callbackCams(data): global camState camState = data.data
def main(): print "INITIALIZING TORSO NODE IN SIMULATION MODE BY MARCOSOFT..." ###Connection with ROS global pubGoalReached rospy.init_node("torso") br = tf.TransformBroadcaster() jointStates = JointState() jointStates.name = [ "spine_connect", "waist_connect", "shoulders_connect", "shoulders_left_connect", "shoulders_right_connect" ] jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0] rospy.Subscriber("/hardware/torso/goal_pose", Float32MultiArray, callbackGoalPose) rospy.Subscriber("/hardware/torso/goal_rel_pose", Float32MultiArray, callbackRelPose) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1) pubCurrentPose = rospy.Publisher("/hardware/torso/current_pose", Float32MultiArray, queue_size=1) pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached", Bool, queue_size=1) loop = rospy.Rate(30) global goalSpine global goalWaist global goalShoulders global spine global waist global shoulders global newGoal spine = 0.10 waist = 0 shoulders = 0 goalSpine = spine goalWaist = waist goalShoulders = shoulders speedSpine = 0.005 speedWaist = 0.1 speedShoulders = 0.1 msgCurrentPose = Float32MultiArray() msgGoalReached = Bool() msgCurrentPose.data = [0, 0, 0] newGoal = False while not rospy.is_shutdown(): deltaSpine = goalSpine - spine deltaWaist = goalWaist - waist deltaShoulders = goalShoulders - shoulders if deltaSpine > speedSpine: deltaSpine = speedSpine if deltaSpine < -speedSpine: deltaSpine = -speedSpine if deltaWaist > speedWaist: deltaWaist = speedWaist if deltaWaist < -speedWaist: deltaWaist = -speedWaist if deltaShoulders > speedShoulders: deltaShoulders = speedShoulders if deltaShoulders < -speedShoulders: deltaShoulders = -speedShoulders spine += deltaSpine waist += deltaWaist shoulders += deltaShoulders jointStates.header.stamp = rospy.Time.now() jointStates.position = [ spine, waist, shoulders, -shoulders, -shoulders ] pubJointStates.publish(jointStates) msgCurrentPose.data[0] = spine msgCurrentPose.data[1] = waist msgCurrentPose.data[2] = shoulders pubCurrentPose.publish(msgCurrentPose) if newGoal and abs(goalSpine - spine) < 0.02 and abs( goalWaist - waist) < 0.05 and abs(goalShoulders - shoulders) < 0.05: newGoal = False msgGoalReached.data = True pubGoalReached.publish(msgGoalReached) loop.sleep()
j['linear_x'] = -NOW_SPEED if hat[0] == 1: j['angular_z'] = -0.3 if hat[0] == -1: j['angular_z'] = 0.3 if joystick.get_button(2) == 1: # button B : shutdowwn print("shutdown!!!!!!!!") j = { 'linear_x': 0.0, 'linear_y': 0.0, 'linear_z': 0.0, 'angular_x': 0.0, 'angular_y': 0.0, 'angular_z': 0.0, } b = Bool() b.data = True pub.publish(b) # only send msgs when the input is not none if j['linear_x'] != 0.0 or j['angular_z'] != 0.0: sendman.send(j) print(j) clock.tick(20) # Close the window and quit. # If you forget this line, the program will 'hang' # on exit if running from IDLE. pygame.quit()
def number_recognition_node(): global flag global audioBuffer sample = None stop = False global THRESHOLD_COEF THRESHOLD_COEF = 0 rospy.init_node('number_recognition_node') pub = rospy.Publisher('/audio/detected_numbers', String, queue_size=1) threspub = rospy.Publisher('/audio/threshold_up', Bool, queue_size=1) rospy.Subscriber('/audio/mic_samples', Int16MultiArray, audio_callback) rospy.Subscriber('/audio/detection_flag', Bool, flag_callback) rospy.Subscriber('/audio/threshold_val', Int16, threshold_callback) #Block samples / second rate = rospy.Rate(int((RATE / BLOCKSIZE))) #Creates folder for image files print(str(flag)) if not os.path.exists(sys.path[0] + '/tmp/tmp'): os.makedirs(sys.path[0] + '/tmp/tmp/') #Constantly publishes number detected while not rospy.is_shutdown(): #If the audio recognition flag is up, starts recognition if flag and audioBuffer != []: #Published message message = String() #Starts defining threshold value with 20 samples print('Defining volume threshold..') prevAudioBuffer = [] while len(prevAudioBuffer) < 20 and not rospy.is_shutdown(): rate.sleep() if audioBuffer != []: prevAudioBuffer.append(audioBuffer.pop(0)) THRESHOLD, THRESHOLD_O = def_threshold(prevAudioBuffer) print('Start talking') #pub.publish(String('Start talking')) # Wait until voice detected. Once detected, breaks the loop # Also saves up to n prevSamples samples before threshold is reached prevAudioBuffer = [] while True and not rospy.is_shutdown(): rate.sleep() if audioBuffer != []: sample = audioBuffer.pop(0) prevAudioBuffer.append(sample) if len(prevAudioBuffer) > MAX_SILENCE_START + 5: prevAudioBuffer.pop(0) threspub.publish(Bool(False)) #Test 5 last samples with threshold to detect input if input_detected_start(prevAudioBuffer, THRESHOLD): break #print('Input detected') threspub.publish(Bool(True)) if rospy.is_shutdown(): break #Once an input is detected, threshold is adjusted down THRESHOLD = THRESHOLD_O #Output wave file is oppened output_wf = wave.open(sys.path[0] + '/myNumber.wav', 'wb') output_wf.setframerate(RATE) output_wf.setsampwidth(WIDTH) output_wf.setnchannels(CHANNELS) #All samples are saved while len(prevAudioBuffer) > 1: output_wf.writeframes(save_sample(prevAudioBuffer.pop(0))) #Starts recording until sound is under threshold for a fixed time silence_count = 0 for n in range(0, int(LEN / BLOCKSIZE)): #Takes current sample and saves it output_wf.writeframes(save_sample(sample)) #Waiting for next sample while audioBuffer == [] and not rospy.is_shutdown(): pass #Takes new sample sample = audioBuffer.pop(0) #Verifies volume of new sample for MAX_SILENCE_END samples up to LEN seconds if not input_detected_end(sample, THRESHOLD): silence_count += 1 if silence_count == MAX_SILENCE_END: print('Input stopped') break else: silence_count = 0 threspub.publish(Bool(False)) #Closes WAV file output_wf.close() #Cleans audio buffer audioBuffer = [] #CNN prediction try: to_png(sys.path[0] + '/myNumber.wav', sys.path[0] + '/tmp/tmp/myNumber.png') message.data = str(predict(sys.path[0] + '/myNumber.png')) print('Predicted as: %s' % (message.data)) if message.data != "None": message.data = "Predicted as: " + message.data pub.publish(message) except: print('Short sample') #print("wait") rate.sleep()
def SetPublishLogger(self, allowPublishLogging): self.logger.SetPublishLogger(self._to_cpp(Bool(allowPublishLogging)))
def stopSim(self): self.stopSimPub.publish(Bool(data=True))
def callback(data): button = Bool() button.data = data.buttons[1] pub.publish(button)
def synchronousSimulation(self, mode): _bool = Bool(data=mode) self.enableSyncModePub.publish(_bool) self._counter = 0
lightPubs = {} addressMap = {} for robot in robotNames: key = str((len(robotKeyNameMap) + 1) % 10) robotKeyNameMap[key] = robot velPubs[key] = rospy.Publisher(robot + '/cmd_vel_relay', Twist, queue_size=1) commPubs[key] = rospy.Publisher(robot + '/comm', String, queue_size=1) selPubs[key] = rospy.Publisher(robot + '/select', Bool, queue_size=1) lightPubs[key] = rospy.Publisher(robot + '/light', Bool, queue_size=1) addressMap[key] = robotAddressMap[robot] if len(robotKeyNameMap) == 10: break currentRobotKey = '1' flag = Bool() flag.data = True selPubs[currentRobotKey].publish(flag) speed = rospy.get_param("~speed", 0.5) turn = rospy.get_param("~turn", 1.0) x = 0 y = 0 z = 0 th = 0 status = 0 try: print(msg) print('--------------------------') print('Robot List:')
def main(portName1, simulated): print "INITIALIZING TORSO..." ###Connection with ROS rospy.init_node("torso") jointStates = JointState() jointStates.name = [ "spine_connect", "waist_connect", "shoulders_connect", "shoulders_left_connect", "shoulders_right_connect" ] jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0] pubTorsoPos = rospy.Publisher("/hardware/torso/current_pose", Float32MultiArray, queue_size=1) pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached", Bool, queue_size=1) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1) pubStop = rospy.Publisher("robot_state/stop", Empty, queue_size=1) pubEmergencyStop = rospy.Publisher("robot_state/emergency_stop", Bool, queue_size=1) subRelativeHeight = rospy.Subscriber("/hardware/torso/goal_rel_pose", Float32MultiArray, callbackRelative) subAbsoluteHeight = rospy.Subscriber("/hardware/torso/goal_pose", Float32MultiArray, callbackAbsolute) subStop = rospy.Subscriber("robot_state/stop", Empty, callbackStop) subEmergencyStop = rospy.Subscriber("robot_state/emergency_stop", Bool, callbackEmergency) subTorsoUp = rospy.Subscriber("/hardware/torso/torso_up", String, callbackTorsoUp) subTorsoDown = rospy.Subscriber("/hardware/torso/torso_down", String, callbackTorsoDown) rate = rospy.Rate(ITER_RATE) global valueRel global valueAbs global absH global relH global stop global torsoUp global torsoDown global eme_stop global new_eme_msg_recv valueAbs = False valueRel = False torsoUp = False torsoDown = False torsoPos = 0 bumper = 0 stop = False msgCurrentPose = Float32MultiArray() msgGoalReached = Bool() msgCurrentPose.data = [0, 0, 0] msgMotor = None initTimeMtrMsg = datetime.now() initTimeSnrMsg = datetime.now() timeoutSnr = 0 timeoutMtr = 0 ArdIfc = comm.Comm(portName1) msgSensor = comm.Msg(comm.ARDUINO_ID, comm.MOD_SENSORS, comm.OP_GETCURRENTDIST, [], 0) ArdIfc.send(msgSensor) goalPose = 0 new_eme_msg_recv = False eme_stop = Bool() eme_stop.data = False while not rospy.is_shutdown(): try: timeoutSnr = datetime.now() - initTimeSnrMsg if timeoutSnr.microseconds > MSG_SENSOR_TIMEOUT: ArdIfc.send(msgSensor) initTimeSnrMsg = datetime.now() newMsg = ArdIfc.recv() if newMsg != None: if newMsg.mod == comm.MOD_SENSORS: if newMsg.op == comm.OP_GETCURRENTDIST: torsoPos = newMsg.param[0] #rospy.loginfo("Torso-> Arduino ack GET CURRENT DIST msg received.") if newMsg.mod == comm.MOD_SYSTEM: if newMsg.op == comm.OP_PING: rospy.loginfo("Torso-> Arduino ack PING msg received.") if newMsg.op == comm.OP_STOP: rospy.loginfo( "Torso-> Arduino Emercenty STOP system received. " ) if eme_stop.data != bool(newMsg.param[0]): eme_stop.data = newMsg.param[0] pubEmergencyStop.publish(eme_stop) if newMsg.mod == comm.MOD_MOTORS: if newMsg.op == comm.OP_SETTORSOPOSE: msgMotor_ack_received = True rospy.loginfo( "Torso-> Arduino ack SETTORSOPOSE msg received.") if newMsg.op == comm.OP_GOUP: msgMotor_ack_received = True rospy.loginfo("Torso-> Arduino ack GOUP msg received.") if newMsg.op == comm.OP_GODOWN: msgMotor_ack_received = True rospy.loginfo( "Torso-> Arduino ack GODOWN msg received.") if newMsg.op == comm.OP_STOP_MOTOR: msgMotor_ack_received = True #rospy.loginfo("Torso-> Arduino ack STOP MOTOR msg received.") #until ack received timeoutMtr = datetime.now() - initTimeMtrMsg if msgMotor != None and timeoutMtr.microseconds > MSG_MOTOR_TIMEOUT and not msgMotor_ack_received: ArdIfc.send(msgMotor) initTimeMtrMsg = datetime.now() if valueAbs and not eme_stop.data and absH >= DIST_LIM_INF and absH <= DIST_LIM_SUP: msgMotor_ack_received = False msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_SETTORSOPOSE, int(absH), 1) ArdIfc.send(msgMotor) valueAbs = False goalPose = absH initTimeMtrMsg = datetime.now() elif valueRel and not eme_stop.data and ( torsoPos + relH) >= DIST_LIM_INF and ( torsoPos + relH) <= DIST_LIM_SUP: msgMotor_ack_received = False absCalH = torsoPos + relH msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_SETTORSOPOSE, int(absCalH), 1) ArdIfc.send(msgMotor) goalPose = absCalH valueRel = False initTimeMtrMsg = datetime.now() elif (valueAbs and (absH < DIST_LIM_INF or absH > DIST_LIM_SUP)) or ( valueRel and (torsoPos + relH > DIST_LIM_SUP or torsoPos + relH < DIST_LIM_INF)): rospy.logerr("Torso-> Can not reach te position.") valueAbs = False valueRel = False goalPose = torsoPos elif torsoUp and not eme_stop.data: rospy.loginfo("Torso-> Moving torso up.") msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_GOUP, [], 0) ArdIfc.send(msgMotor) torsoUp = False msgMotor_ack_received = False initTimeMtrMsg = datetime.now() goalPose = torsoPos elif torsoDown and not eme_stop.data: rospy.loginfo("Torso-> Moving torso down.") msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_GODOWN, [], 0) ArdIfc.send(msgMotor) torsoDown = False msgMotor_ack_received = False initTimeMtrMsg = datetime.now() goalPose = torsoPos elif eme_stop.data and new_eme_msg_recv: rospy.loginfo("Torso-> Stop message.") msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS, comm.OP_STOP_MOTOR, [], 0) ArdIfc.send(msgMotor) msgMotor_ack_received = False initTimeMtrMsg = datetime.now() new_eme_msg_recv = False torsoDown = False torsoUp = False valueAbs = False valueRel = False jointStates.header.stamp = rospy.Time.now() jointStates.position = [(torsoPos - TORSO_ADJUSTMENT) / 100.0, 0.0, 0.0, 0.0, 0.0] pubJointStates.publish(jointStates) msgCurrentPose.data[0] = (torsoPos - TORSO_ADJUSTMENT) / 100.0 msgCurrentPose.data[1] = 0.0 msgCurrentPose.data[2] = 0.0 pubTorsoPos.publish(msgCurrentPose) msgGoalReached.data = abs(goalPose - torsoPos) < THR_DIFF_POS pubGoalReached.publish(msgGoalReached) rate.sleep() except: rospy.logerr( "Torso-> Oopps...we have some issues in this node :( ")
def __init__(self): rospy.Subscriber("rob_id", rob_id, self.publisher_id) self.id_pub_1 = rospy.Publisher(sys.argv[1] + "/robot_id", Bool, queue_size=1) self.id_pub_2 = rospy.Publisher(sys.argv[2] + "/robot_id", Bool, queue_size=1) self._robot_id = Bool() self._robot_id.data = True
def reset_joint_positions(self): msg = Bool() msg.data = True self.publisher.publish(msg) rospy.loginfo( "Hardware Joint Resetter: Wheel positions reset to zero.")
def requestNewWaypoint(self): req = Bool() req.data = False # Random frontier flag to false self.pub_replan.publish(req)
def find_object(): global pub_find_object pub_find_object.publish(Bool(True))
def publish_state(self): bmsg = Bool() bmsg.data = self.recording self.rec_pub.publish(bmsg)
def publish_dbw_status(self, data): self.publishers['dbw_status'].publish(Bool(data))
#========================# PI = 3.141592653589793 ardrone = States() states = States() states.x = 0.0 states.y = 0.0 states.u = 0.0 states.v = 0.0 g = 9.81 L = 1 pen_in = PointStamped() rate = 200 # Hz pub_states = rospy.Publisher('/cortex_raw', Cortex, queue_size=1) start_time = 0 controller_status = Bool() controller_status.data = False #==================# # Get States # #==================# def GetStates(I): global ardrone ardrone = I #=============================# # Get Controller Status # #=============================#
stop_wait = time.time() + wait_open path = [] while not rospy.is_shutdown(): r = rospy.Rate(1) # 3hz r.sleep() # If lidar detects right door, go in. Else after some time, go left. if elevator == "right": rospy.set_param('/elevator', 'right') inside1 = Twist() inside1.linear.x, inside1.linear.y = 53.4, 3.9 inside2 = Twist() inside2.linear.x, inside2.linear.y, inside2.angular.z = 53.4, 3.9, 3.14 path = [inside1, inside2] break elif time.time() >= stop_wait: # left if right didn't open rospy.set_param('/elevator', 'left') outside = Twist() outside.linear.x, outside.linear.y = 51.5, 6.8 inside1 = Twist() inside1.linear.x, inside1.linear.y = 53.4, 6.8 inside2 = Twist() inside2.linear.x, inside2.linear.y, inside2.angular.z = 53.4, 7.0, 3.14 path = [outside, inside1, inside2] break wp_pub = WaypointPublisher(path) while not wp_pub.done and not rospy.is_shutdown(): r = rospy.Rate(10) r.sleep() done_pub.publish(Bool(data=True))
def update(self): self.state_lock.acquire() state = copy.copy(self.state_obj) self.state_lock.release() if state is None: return self.program_runtime_state = state.runtime_state self.parse_and_publish_state(state) if self.program_runtime_state != hp.UR_RUNNING_FLAG: self.robot_ready = False self.pub_robot_ready.publish(Bool(False)) self.ping_id = [] self.ros_rate.sleep() return if not self.robot_ready and self.ping_id == []: self.ping_id = int(state.output_int_register_1 != 1) hp.list_to_input(self.ping_obj, self.ping_id, 1, hp.PING_INT_REGISTER, "int") self.con.send(self.ping_obj) elif not self.robot_ready and self.ping_id == state.output_int_register_1: self.robot_ready = True self.pub_robot_ready.publish(Bool(True)) print "ROBOT READY" self.command_lock.acquire() mode = self.mode setp = self.setp avtr = self.avtr setp2 = self.setp2 enable = self.enable self.command_lock.release() if self.mode_prev != mode: self.start_new_mode(mode) self.mode_prev = mode # do the update for each mode if mode == hp.STANDBY_MODE: # STANDBY MODE hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER, "int") hp.list_to_input(self.enable_obj, enable, 1, hp.ENABLE_INT_REGISTER, "int") self.con.send(self.mode_obj) self.con.send(self.enable_obj) elif mode == hp.FREEDRIVE_MODE: # FREEDRIVE MODE hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER, "int") hp.list_to_input(self.enable_obj, enable, 1, hp.ENABLE_INT_REGISTER, "int") self.con.send(self.mode_obj) self.con.send(self.enable_obj) elif mode == hp.WAYPOINT_JOINT_MODE: # WAYPOINT JOINT MODE hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER, "int") hp.list_to_input(self.setp_obj, setp, 6, hp.SETP_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.avtr_obj, avtr, 4, hp.AVTR_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.enable_obj, enable, 1, hp.ENABLE_INT_REGISTER, "int") # send new setpoint self.con.send(self.mode_obj) self.con.send(self.setp_obj) self.con.send(self.avtr_obj) self.con.send(self.enable_obj) elif mode == hp.POSITION_CONTROL_JOINT_MODE: # JOINT POSITION CONTROL MODE hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER, "int") hp.list_to_input(self.setp_obj, setp, 6, hp.SETP_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.servoj_params_obj, self.servoj_params, 3, hp.SERVOJ_PARAMS_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.enable_obj, enable, 1, hp.ENABLE_INT_REGISTER, "int") # send new setpoint self.con.send(self.mode_obj) self.con.send(self.setp_obj) self.con.send(self.servoj_params_obj) self.con.send(self.enable_obj) elif mode == hp.VELOCITY_CONTROL_JOINT_MODE: # JOINT SPEED CONTROL MODE hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER, "int") hp.list_to_input(self.setp_obj, setp, 6, hp.SETP_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.avtr_obj, avtr, 4, hp.AVTR_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.enable_obj, enable, 1, hp.ENABLE_INT_REGISTER, "int") # send new setpoint self.con.send(self.mode_obj) self.con.send(self.setp_obj) self.con.send(self.avtr_obj) self.con.send(self.enable_obj) elif mode == hp.POSITION_CONTROL_CARTESIAN_MODE: # CARTESIAN POSITION CONTROL MODE hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER, "int") hp.list_to_input(self.setp_obj, setp, 6, hp.SETP_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.servoj_params_obj, self.servoj_params, 3, hp.SERVOJ_PARAMS_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.enable_obj, enable, 1, hp.ENABLE_INT_REGISTER, "int") # send new setpoint self.con.send(self.mode_obj) self.con.send(self.setp_obj) self.con.send(self.servoj_params_obj) self.con.send(self.enable_obj) elif mode == hp.IMPEDANCE_CONTROL_MODE: # IMPEDANCE CONTROL MODE hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER, "int") hp.list_to_input(self.setp_obj, setp, 6, hp.SETP_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.setp2_obj, setp2, 6, hp.SETP2_DOUBLE_REGISTER_OFFSET, "double") hp.list_to_input(self.enable_obj, enable, 1, hp.ENABLE_INT_REGISTER, "int") # send new setpoint self.con.send(self.mode_obj) self.con.send(self.setp_obj) self.con.send(self.setp2_obj) self.con.send(self.enable_obj) self.ros_rate.sleep()
def main(): # NOR gate RS flip flop example # Input topics /S and /R are active high (true is logic 1 and false is logic 0) # Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1) # Input/Output table # S: false R: false | Q: no change Q_not: no change # S: true R: false | Q: false Q_not: true # S: false R: true | Q: true Q_not: false # S: true R: true | Q: invalid Q_not: invalid builder = DiagramBuilder() sys_ros_interface = builder.AddSystem(RosInterfaceSystem()) qos = QoSProfile(depth=10) sys_pub_Q = builder.AddSystem( RosPublisherSystem(Bool, "Q", qos, sys_ros_interface.get_ros_interface())) sys_pub_Q_not = builder.AddSystem( RosPublisherSystem(Bool, "Q_not", qos, sys_ros_interface.get_ros_interface())) sys_sub_S = builder.AddSystem( RosSubscriberSystem(Bool, "S", qos, sys_ros_interface.get_ros_interface())) sys_sub_R = builder.AddSystem( RosSubscriberSystem(Bool, "R", qos, sys_ros_interface.get_ros_interface())) sys_nor_gate_1 = builder.AddSystem(NorGate()) sys_nor_gate_2 = builder.AddSystem(NorGate()) sys_memory = builder.AddSystem(Memory(Bool())) builder.Connect(sys_nor_gate_1.GetOutputPort('Q'), sys_memory.get_input_port(0)) builder.Connect( sys_sub_S.get_output_port(0), sys_nor_gate_1.GetInputPort('A'), ) builder.Connect( sys_nor_gate_2.GetOutputPort('Q'), sys_nor_gate_1.GetInputPort('B'), ) builder.Connect( sys_memory.get_output_port(0), sys_nor_gate_2.GetInputPort('A'), ) builder.Connect( sys_sub_R.get_output_port(0), sys_nor_gate_2.GetInputPort('B'), ) builder.Connect(sys_nor_gate_1.GetOutputPort('Q'), sys_pub_Q.get_input_port(0)) builder.Connect(sys_nor_gate_2.GetOutputPort('Q'), sys_pub_Q_not.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator_context = simulator.get_mutable_context() simulator.set_target_realtime_rate(1.0) while True: simulator.AdvanceTo(simulator_context.get_time() + 0.1)
def __init__(self, quad_name, environment="gazebo", recording_options=None, load_options=None, use_ekf=False, rdrv=None, plot=False, reset_experiment=False): if recording_options is None: recording_options = {"recording": False} # If on a simulation environment, figure out if physics are slowed down if environment == "gazebo": try: get_gazebo_physics = rospy.ServiceProxy('/gazebo/get_physics_properties', GetPhysicsProperties) resp = get_gazebo_physics() physics_speed = resp.max_update_rate * resp.time_step rospy.loginfo("Physics running at %.2f normal speed" % physics_speed) except rospy.ServiceException as e: print("Service call failed: %s" % e) physics_speed = 1 else: physics_speed = 1 self.physics_speed = physics_speed self.environment = environment self.plot = plot self.recording_options = recording_options # Control at 50 (sim) or 60 (real) hz. Use time horizon=1 and 10 nodes self.n_mpc_nodes = rospy.get_param('~n_nodes', default=10) self.t_horizon = rospy.get_param('~t_horizon', default=1.0) self.control_freq_factor = rospy.get_param('~control_freq_factor', default=5 if environment == "gazebo" else 6) self.opt_dt = self.t_horizon / (self.n_mpc_nodes * self.control_freq_factor) # Load trained GP model if load_options is not None: rospy.loginfo("Attempting to load GP model from:\n git: {}\n name: {}\n meta: {}".format( load_options["git"], load_options["model_name"], load_options["params"])) pre_trained_models = load_pickled_models(model_options=load_options) if pre_trained_models is None: rospy.logwarn("Model parameters specified did not match with any pre-trained GP") else: pre_trained_models = None self.pre_trained_models = pre_trained_models self.git_v = load_options["git"] if self.pre_trained_models is not None: rospy.loginfo("Successfully loaded GP model") self.model_name = load_options["model_name"] elif rdrv is not None: self.model_name = "rdrv" else: self.model_name = "Nominal" # Initialize GP MPC for point tracking self.gp_mpc = ROSGPMPC(self.t_horizon, self.n_mpc_nodes, self.opt_dt, quad_name=quad_name, point_reference=False, gp_models=pre_trained_models, rdrv=rdrv) # Last state obtained from odometry self.x = None # Elapsed time between two recordings self.last_update_time = time.time() # Last references. Use hovering activation as input reference self.last_x_ref = None self.last_u_ref = None # Reference trajectory variables self.x_ref = None self.t_ref = None self.u_ref = None self.current_idx = 0 self.quad_trajectory = None self.quad_controls = None self.w_control = None # Provisional reference for "waiting_for_reference" hovering mode self.x_ref_prov = None # To measure optimization elapsed time self.optimization_dt = 0 # Thread for MPC optimization self.mpc_thread = threading.Thread() # Trajectory tracking experiment. Dims: seed x av_v x n_samples if reset_experiment: self.metadata_dict = {} else: self.metadata_dict, _, _, _ = load_past_experiments() self.mse_exp = np.zeros((0, 0, 0, 1)) self.t_opt = np.zeros((0, 0, 0)) self.mse_exp_v_max = np.zeros((0, 0)) self.ref_traj_name = "" self.ref_v = 0 self.run_traj_counter = 0 # Keep track of status of MPC object self.odom_available = False # Binary variable to run MPC only once every other odometry callback self.optimize_next = True # Binary variable to completely skip an odometry if in flying arena self.skip_next = False # Remember the sequence number of the last odometry message received. self.last_odom_seq_number = 0 # Measure if trajectory starting point is reached self.x_initial_reached = False # Variables for recording mode self.recording_warmup = True self.x_pred = None self.w_opt = None # Odometry estimate for GP. None by default if same odometry as control reference should be used self.gp_odom = None # Get recording file and directory blank_recording_dict = make_record_dict(state_dim=13) if recording_options["recording"]: record_raw_optitrack = recording_options["record_raw_optitrack"] overwrite = recording_options["overwrite"] metadata = {self.environment: "default"} rec_dict, rec_file = get_record_file_and_dir( blank_recording_dict, recording_options, simulation_setup=metadata, overwrite=overwrite) # If flying with the optitrack system, also record raw optitrack estimates if self.environment == "flying_room" or self.environment == 'arena' and record_raw_optitrack: rec_dict_raw = make_raw_optitrack_dict() metadata = {self.environment: "optitrack_raw"} rec_dict_raw, rec_file_raw = get_record_file_and_dir( rec_dict_raw, recording_options, simulation_setup=metadata, overwrite=overwrite) else: rec_dict_raw = rec_file_raw = None else: record_raw_optitrack = False rec_dict = rec_file = None rec_dict_raw = rec_file_raw = None self.rec_dict = rec_dict self.rec_file = rec_file self.rec_dict_raw = rec_dict_raw self.rec_file_raw = rec_file_raw self.landing = False self.override_land = False self.ground_level = False self.controller_off = False # Setup node publishers and subscribers. The odometry (sub) and control (pub) topics will vary depending on # which environment is being used ekf_odom_topic = None if self.environment == "gazebo": odom_topic = "/" + quad_name + "/ground_truth/odometry" raw_topic = None elif self.environment == "arena": # Assume arena world setup odom_topic = "/" + quad_name + "/state_estimate" raw_topic = "/vicon/" + quad_name if use_ekf: ekf_odom_topic = "/" + quad_name + "/state_estimate_ekf" else: # Assume real world setup odom_topic = "/" + quad_name + "/state_estimate" raw_topic = "/optitrack/" + quad_name if use_ekf: ekf_odom_topic = "/" + quad_name + "/state_estimate_ekf" land_topic = "/" + quad_name + "/autopilot/land" control_topic = "/" + quad_name + "/autopilot/control_command_input" off_topic = "/" + quad_name + "/autopilot/off" reference_topic = "reference" status_topic = "busy" # Publishers self.control_pub = rospy.Publisher(control_topic, ControlCommand, queue_size=1, tcp_nodelay=True) self.status_pub = rospy.Publisher(status_topic, Bool, queue_size=1) self.off_pub = rospy.Publisher(off_topic, Empty, queue_size=1) # Subscribers self.land_sub = rospy.Subscriber(land_topic, Empty, self.land_callback) self.ref_sub = rospy.Subscriber(reference_topic, ReferenceTrajectory, self.reference_callback) if ekf_odom_topic: # We get a second odometry estimate which is smoothed. Used to evaluate the GP's. self.odom_sub = rospy.Subscriber( odom_topic, Odometry, self.odometry_callback, queue_size=1, tcp_nodelay=True) self.ekf_odom_sub = rospy.Subscriber( ekf_odom_topic, Odometry, self.ekf_odom_callback, queue_size=1, tcp_nodelay=True) else: self.odom_sub = rospy.Subscriber( odom_topic, Odometry, self.odometry_callback, queue_size=1, tcp_nodelay=True) if raw_topic is not None and record_raw_optitrack: self.raw_sub = rospy.Subscriber(raw_topic, PoseStamped, self.raw_odometry_callback) rate = rospy.Rate(1) while not rospy.is_shutdown(): # Publish if MPC is busy with a current trajectory msg = Bool() msg.data = not (self.x_ref is None and self.odom_available) self.status_pub.publish(msg) rate.sleep()