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()
global ff_pwm ff_pwm = 0 # int min_motor_position = 990 max_motor_position = 3525 max_motor_velocity = 1023 # signed 10-bit int with units 0.229 rpm/bit if motor_controller is ControlApproach.POSITION or motor_controller is ControlApproach.CASCADE: bag = rosbag.Bag('actual_position.bag', 'w') elif motor_controller is ControlApproach.VELOCITY: bag = rosbag.Bag('actual_velocity.bag', 'w') global bag_data bag_pos = Int32() bag_error = Int32() bag_pwm = Int32() def pen_down(): global pen_pos_up print('\nPEN DOWN...\n') pen_pos_up = False stop_motors() time.sleep(.1) pen_down_msg = SetMotorPosition() pen_down_msg.id = 2 pen_down_msg.position = 1275
#! /usr/bin/env python import rospy from std_msgs.msg import Int32 rospy.init_node('topic_publisher') pub = rospy.Publisher('/counter', Int32, queue_size=1) #create a publsher object that publish on the /counter topic rate = rospy.Rate(2) count = Int32() #create a var of type Int32 count.data = 0 while not rospy.is_shutdown(): #until someone stops #the program execution pub.publish(count) count.data += 1 rate.sleep()
def pr2_mover(object_list): '''*********************************************************************** # TODO: Initialize variables ***********************************************************************''' escena = 3 yaml_filename_output = "output_0.yaml" test_scene_num = Int32() test_scene_num.data = escena dict_list = [] '''*********************************************************************** # TODO: Get/Read parameters ***********************************************************************''' object_list_param = rospy.get_param('/object_list') dropbox_list_param = rospy.get_param('/dropbox') '''*********************************************************************** # TODO: Parse parameters into individual variables ***********************************************************************''' for object in object_list: object_name = String() arm_name = String() pick_pose = Pose() place_pose = Pose() #Obtaining name of the object object_name.data = str(object.label) #Obtaining group of the object for item in object_list_param: if item['name'] == object_name.data : object_group = item['group'] # TODO: Rotate PR2 in place to capture side tables for the collision map '''*********************************************************************** # TODO: Get the PointCloud for a given object and obtain it's centroid ***********************************************************************''' points_arr = ros_to_pcl(object.cloud).to_array() cloud = np.mean(points_arr, axis=0)[:3] pick_pose.position.x = np.asscalar(cloud[0]) pick_pose.position.y = np.asscalar(cloud[1]) pick_pose.position.z = np.asscalar(cloud[2]) '''*********************************************************************** # TODO: Create 'place_pose' for the object ***********************************************************************''' for item in dropbox_list_param: if item['group'] == object_group: position = item['position'] place_pose.position.x = position[0] place_pose.position.y = position[1] place_pose.position.z = position[2] '''*********************************************************************** # TODO: Assign the arm to be used for pick_place ***********************************************************************''' if object_group == 'red': arm_name.data = str('left') else: arm_name.data = str('right') '''************************************************************************************************ # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format ************************************************************************************************''' yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) '''*********************************************************************** # TODO: Insert your message variables to be sent as a service request ***********************************************************************''' resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose) print ("Response: ",resp.success) except rospy.ServiceException, e: print "Service call failed: %s"%e
def __init__(self): # Initialize the UAV self.id = input("Drone ID #: ") self.uav_id = "uav" + str(self.id) rospy.init_node("controller" + str(self.id), anonymous=True) self.rate = rospy.Rate( 100 ) # Determine what this value should be to maximize performance in sim and real world scenarios # Define Collision Cylinders # Change these later to a dynamic value that depends on breaking distance so Rc = Rc +Dbr 'breaking distance'. self.reserved_cyl_radius = 2 self.reserved_cyl_vertical = 1 self.blocking_cyl_radius = self.reserved_cyl_radius self.blocking_cyl_vertical = 2 self.conflict_state = {'xy': 'xy-free', 'z': 'z-free'} self.escape_angle = 0 # Setup the cylindrical obstacle diagram (COD) self.COD = {'distance': {}, 'angle': {}, 'z-diff': {}} # Create required publsihers for control self.target_pos_pub = rospy.Publisher( self.uav_id + "/mavros/setpoint_position/local", PoseStamped, queue_size=1) self.target_vel_pub = rospy.Publisher( self.uav_id + "/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=1) # Create required subcribers for information and control self.state_sub = rospy.Subscriber(self.uav_id + "/mavros/state", State, self.state_changed_callback) self.actual_pos_sub = rospy.Subscriber( self.uav_id + "/mavros/local_position/pose", PoseStamped, self.position_callback) self.actual_pos_sub = rospy.Subscriber( self.uav_id + "/mavros/global_position/global", NavSatFix, self.global_pos_changed_callback) self.actual_vel_sub = rospy.Subscriber( self.uav_id + "/mavros/local_position/velocity_local", TwistStamped, self.velocity_callback) self.heading_sub = rospy.Subscriber( self.uav_id + "/mavros/global_position/compass_hdg", Float64, self.heading_changed_callback) self.battery_voltage_sub = rospy.Subscriber( self.uav_id + "/mavros/battery", BatteryState, self.battery_voltage_callback) self.network_state_sub = rospy.Subscriber( "/network_map", Int32, self.network_state_changed_callback) # Create Objects to pair with above pubs and subs self.desired_velocity_object = TwistStamped() self.battery_voltage_object = BatteryState() self.actual_global_pos_object = NavSatFix() self.neighbor_global_position_objects = {} self.desired_pos_object = PoseStamped() self.actual_pos_object = PoseStamped() self.actual_velocity_object = Twist() self.neighbor_velocity_objects = {} self.neighbor_position_objects = {} self.neighbor_global_pos_sub = {} self.num_nodes_in_swarm = Int32() self.mode_object = SetMode() self.neighbor_pos_sub = {} self.neighbor_vel_sub = {} self.heading = Float64() self.state = State() self.currently_avoiding = False self.count = 0 self.set_failsafe_action(2) self.set_param('MPC_ACC_HOR_MAX', 1.0)
def pr2_mover(object_list): # TODO: Initialize variables test_scene_num = Int32() object_name = String() arm_name = String() pick_pose = Pose() place_pose = Pose() dict_list = [] labels = [] centroids = [] # to be list of tuples (x, y, z) # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_list_param = rospy.get_param('/dropbox') # TODO: Parse parameters into individual variables object_list_group = [] object_list_name = [] dropbox_group = [] for item in object_list_param: object_list_name.append(item['name']) object_list_group.append(item['group']) for item in dropbox_list_param: dropbox_group.append(item['group']) test_scene_num.data = 1 # TODO: Loop through the pick list for object in object_list: #Fiding the object group and name object_group = object_list_param[object_list_name.index( object.label)]['group'] object_name.data = str(object.label) #Fiding place_pose and arm name arm_name.data = str( dropbox_list_param[dropbox_group.index(object_group)]['name']) pose = ( dropbox_list_param[dropbox_group.index(object_group)]['position']) place_pose.position = Point(pose[0], pose[1], pose[2]) # Pick_pose points_arr = ros_to_pcl(object.cloud).to_array() centroid = (np.mean(points_arr, axis=0)[:3]) #asscalar solved TypeError: data type not understood pick_pose.position = Point(np.asscalar(centroid[0]), np.asscalar(centroid[1]), np.asscalar(centroid[2])) yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) yaml_filename = 'output_' + str(test_scene_num.data) + '.yaml' send_to_yaml(yaml_filename, dict_list) '''
def pr2_mover(object_list): # TODO: init list to save objs dict_list = [] centroids = [] # to be list of tuples (x, y, z) # TODO: get list of objs form parameter server objects_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') # TODO: create the dictionary of dropbox dropbox = {} for p in dropbox_param: dropbox[p['name']] = p['position'] # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list for obj in objects_param: # TODO: get the name of the obj object_name = String() object_name.data = obj['name'] true_label = object_name.data #set default value of pick_pose pick_pose = Pose() pick_pose.position.x = 0 pick_pose.position.y = 0 pick_pose.position.z = 0 #set orientation to 0 pick_pose.orientation.x = 0 pick_pose.orientation.y = 0 pick_pose.orientation.z = 0 pick_pose.orientation.w = 0 #set place pose orientation to 0 place_pose = Pose() place_pose.orientation.x = 0 place_pose.orientation.y = 0 place_pose.orientation.z = 0 place_pose.orientation.w = 0 for detected_object in object_list: pridicted_label = detected_object.label if pridicted_label == true_label: # TODO: Create 'place_pose' for the object points_arr = ros_to_pcl(detected_object.cloud).to_array() centroids.append(np.mean(points_arr, axis=0)[:3]) pick_pose_np = np.mean(points_arr, axis=0)[:3] pick_pose.position.x = np.asscalar(pick_pose_np[0]) pick_pose.position.y = np.asscalar(pick_pose_np[1]) pick_pose.position.z = np.asscalar(pick_pose_np[2]) print("------") print("centroid is :") print(np.mean(points_arr, axis=0)[:3]) break # TODO: Assign the arm to be used for pick_place arm_name = String() if obj['group'] == 'red': arm_name.data = 'left' elif obj['group'] == 'green': arm_name.data = 'right' else: print "ERROR, group must be green or red!" # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format test_scene_num = Int32() test_scene_num.data = 3 place_pose.position.x = dropbox[arm_name.data][0] place_pose.position.y = dropbox[arm_name.data][1] place_pose.position.z = dropbox[arm_name.data][2] dict_list.append( make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') # TODO: Output your request parameters into output yaml file yaml_filename = "output_" + str(test_scene_num.data) + ".yaml" send_to_yaml(yaml_filename, dict_list)
def tilt_up(self, deg): pa = rospy.get_param("/robot/get_tilt") pn = pa + deg v = Int32(pn) self.blackboard["pub_tilt"].publish(v)
def pr2_mover(object_list, object_table): print("Starting Pick and Place") # set the test scene number TEST_SCENE_NUM = Int32() TEST_SCENE_NUM.data = 1 # get the object centroids object_labels, object_centroids = find_centroids(object_list) # Get/Read parameters object_list_param = rospy.get_param('/object_list') # loop though the object list dict_list = [] for i in range(len(object_list_param)): # Parse parameters into individual variables object_name = String() object_name.data = object_list_param[i]['name'] object_group = object_list_param[i]['group'] # Get the PointCloud for a given object and obtain it's centroid index = find_object_index(object_list_param[i]['name'], object_labels) # -1 means we never found this object if index == -1: continue centroid = object_centroids[index] arm = String() # Assign the arm to be used for pick_place if object_group == "green": arm.data = "right" else: arm.data = "left" # Create 'pick_pose' for the object pick_pose = Pose() pick_pose.position.x = centroid[0] pick_pose.position.y = centroid[1] pick_pose.position.z = centroid[2] + 0.01 # no information in yaml file, or course info, so I am assuming zero pick_pose.orientation.x = 0 pick_pose.orientation.y = 0 pick_pose.orientation.z = 0 # create 'place_pose' for the object place_pose = Pose() # no information in yaml file, or course info, so I am assuming zero place_pose.orientation.x = 0 place_pose.orientation.y = 0 place_pose.orientation.z = 0 # set position based on the target box if object_group == "green": place_pose.position.x = PLACE_GREEN_X place_pose.position.y = PLACE_GREEN_Y place_pose.position.z = PLACE_GREEN_Z else: place_pose.position.x = PLACE_RED_X place_pose.position.y = PLACE_RED_Y place_pose.position.z = PLACE_RED_Z yaml_dict = make_yaml_dict(TEST_SCENE_NUM, arm, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # send the full collision point cloud collision_map = [] collision_list = collision_cloud.getCollisionClouds() table_list = pointCloudToList(object_table) object_collision_list = create_collision_list_from_objects(object_list, index) collision_map.extend(table_list) collision_map.extend(collision_list) collision_map.extend(object_collision_list) collision_map_pcl = pcl.PointCloud() collision_map_pcl.from_list(collision_map) color_list = get_color_list(2) collision_map_pcl = XYZ_to_XYZRGB(collision_map_pcl, color_list[0]) collision_map_ros = pcl_to_ros(collision_map_pcl) sensor_msg_pub.publish(collision_map_ros) # # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') # Call the pick_place service try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) resp = pick_place_routine(TEST_SCENE_NUM, object_name, arm, pick_pose, place_pose) print ("Response: ", resp.success) #print ("HELLO WORLD") except rospy.ServiceException, e: print "Service call failed: %s" % e
def ros_deepsort_callback(self, ros_data): start = time.time() #convert ros compressed image message to opencv if self.args.service: np_arr = np.fromstring(ros_data.img.data, np.uint8) elif self.args.img_topic: np_arr = np.fromstring(ros_data.data, np.uint8) ori_im = cv2.imdecode(np_arr, flags=cv2.IMREAD_COLOR) im = cv2.cvtColor(ori_im, cv2.COLOR_BGR2RGB) #skip frame per frame interval self.idx_frame += 1 if self.idx_frame % self.args.frame_interval: return # do detection bbox_xywh, cls_conf, cls_ids = self.detector(im) # select person class mask = cls_ids == 0 bbox_xywh = bbox_xywh[mask] # bbox dilation just in case bbox too small, delete this line if using a better pedestrian detector bbox_xywh[:, 3:] *= 1.2 cls_conf = cls_conf[mask] # do tracking outputs = self.deepsort.update(bbox_xywh, cls_conf, im, tracking_target=self.idx_tracked) # if detection present draw bounding boxes if len(outputs) > 0: bbox_tlwh = [] self.bbox_xyxy = outputs[:, :4] # detection indices self.identities = outputs[:, -1] ori_im = draw_boxes(ori_im, self.bbox_xyxy, self.identities) for bb_xyxy in self.bbox_xyxy: bbox_tlwh.append(self.deepsort._xyxy_to_tlwh(bb_xyxy)) end = time.time() #draw frame count font = cv2.FONT_HERSHEY_SIMPLEX bottomLeftCornerOfText = (10, 500) fontScale = 1 fontColor = (255, 255, 255) lineType = 2 frame_count = ("Frame no: %d" % self.idx_frame) cv2.putText(ori_im, frame_count, bottomLeftCornerOfText, font, fontScale, fontColor, lineType) #draw tracking number if self.idx_tracked: tracking_str = ("Tracking: %d" % self.idx_tracked) else: tracking_str = ("Tracking: None") bottomLeftCornerOfText = (10, 550) cv2.putText(ori_im, tracking_str, bottomLeftCornerOfText, font, fontScale, fontColor, lineType) #### Create CompressedIamge #### msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', ori_im)[1]).tostring() # Publish new image self.image_pub.publish(msg) if self.args.save_results: self.writer.write(ori_im) # logging self.logger.info("frame: {}, time: {:.03f}s, fps: {:.03f}, detection numbers: {}, tracking numbers: {}" \ .format(self.idx_frame, end-start, 1/(end-start), bbox_xywh.shape[0], len(outputs))) """publishing to topics""" #publish detection identities identity_msg = Int32MultiArray(data=self.identities) self.detections_pub.publish(identity_msg) #publish if target present if len(outputs) == 0 or self.idx_tracked is None: self.target_present_pub.publish(Bool(False)) elif len(outputs) > 0 and self.idx_tracked: self.target_present_pub.publish(Bool(True)) #publish angle and xy data if self.idx_tracked is not None: x_center = (self.bbox_xyxy[0][0] + self.bbox_xyxy[0][2]) / 2 y_center = (self.bbox_xyxy[0][1] + self.bbox_xyxy[0][3]) / 2 pixel_per_angle = im.shape[1] / self.camera_fov x_center_adjusted = x_center - (im.shape[1] / 2) #print(x_center_adjusted) angle = x_center_adjusted / pixel_per_angle #print(angle) if self.args.img_topic: self.bbox_pub.publish(x_center, y_center, 0) self.angle_pub.publish(angle) self.target_indice_pub.publish(Int32(self.idx_tracked)) msg = Point() msg.x = x_center msg.y = y_center msg.z = 0 return msg #publish if target initialized else: self.target_indice_pub.publish(Int32(self.idx_tracked)) msg = Point() msg.x = 0 msg.y = 0 msg.z = 0 return msg
def pan_left(self, deg): pa = rospy.get_param("/robot/get_pan") pn = pa + deg v = Int32(pn) self.blackboard["pub_pan"].publish(v)
def pr2_mover(object_list): world = 1 dropbox = rospy.get_param('/dropbox') box = {} for i, db in enumerate(dropbox): data = {} data["arm"] = db['name'] data["position"] = Pose() data["position"].position.x = db['position'][0] data["position"].position.y = db['position'][1] data["position"].position.z = db['position'][2] box[db['group']] = data # TODO: Initialize variables object_list_param = rospy.get_param('/object_list') # TODO: Get/Read parameters # TODO: Parse parameters into individual variables # TODO: Rotate PR2 in place to capture side tables for the collision map labels = [] centroids = {} for object in object_list: labels.append(object.label) points_arr = ros_to_pcl(object.cloud).to_array() centroids[object.label] = Pose() position = (np.mean(points_arr, axis=0)[:3]) centroids[object.label].position.x = np.asscalar(position[0]) centroids[object.label].position.y = np.asscalar(position[1]) centroids[object.label].position.z = np.asscalar(position[2]) dict_list = [] for i, object in enumerate(object_list_param): # TODO: Loop through the pick list obj_name = object['name'] obj_group = object['group'] test_scene_num = Int32() test_scene_num.data = world object_name = String() object_name.data = obj_name arm_name = String() arm_name.data = box[obj_group]['arm'] pick_pose = centroids[obj_name] place_pose = box[obj_group]['position'] # TODO: Get the PointCloud for a given object and obtain it's centroid # TODO: Create 'place_pose' for the object # TODO: Assign the arm to be used for pick_place # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # Wait for 'pick_place_routine' service to come up ''' rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) print ("Response: ",resp.success) except rospy.ServiceException, e: print "Service call failed: %s"%e ''' # TODO: Output your request parameters into output yaml file send_to_yaml("output" + str(world) + ".yaml", dict_list)
def pr2_mover(object_list): # TODO: Initialize variables object_list_param = [] # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') #rospy.loginfo('object_list_param {}'.format(object_list_param)) # TODO: Parse parameters into individual variables labels = [] centroids = [] # to be list of tuples (x, y, z) detected_object_dict = {} for object in object_list: labels.append(object.label) points_arr = ros_to_pcl(object.cloud).to_array() centroids.append(np.mean(points_arr, axis=0)[:3]) if not object.label in detected_object_dict: detected_object_dict.update( {object.label: np.mean(points_arr, axis=0)[:3]}) #rospy.loginfo('centroid {}'.format(np.mean(points_arr, axis=0))) #print(detected_object_dict) test_scene_num = Int32() test_scene_num.data = rospy.get_param("test_scene_num") dropbox_dict = {} for i in range(len(dropbox_param)): dropbox_dict.update({ dropbox_param[i]['group']: [dropbox_param[i]['name'], dropbox_param[i]['position']] }) # TODO: Rotate PR2 in place to capture side tables for the collision map #rate = rospy.Rate(1000) #pr2_world_joint_pub.publish(math.pi/2) #rate.sleep() ##collision_map_pub.publish(pcl_msg) #pr2_world_joint_pub.publish(-math.pi/2) #rate.sleep() #rate.sleep() #pr2_world_joint_pub.publish(0.0) #rate.sleep() # TODO: Loop through the pick list dict_list = [] for i in range(len(object_list_param)): # TODO: Get the PointCloud for a given object and obtain it's centroid object_name = String() object_name.data = object_list_param[i]['name'] pick_pose = Pose() centroid = detected_object_dict.get(object_name.data, [0, 0, 0]) pick_pose.position.x = float(centroid[0]) pick_pose.position.y = float(centroid[1]) pick_pose.position.z = float(centroid[2]) #pick_pose.orientation = [0,0,0,0] #rospy.loginfo('detected_object_dict {}'.format(detected_object_dict.get(object_name.data, [0,0,0]))) # TODO: Create 'place_pose' for the object place_pose = Pose() dropbox_pos = dropbox_dict.get(object_list_param[i]['group'])[1] place_pose.position.x = float(dropbox_pos[0]) place_pose.position.y = float(dropbox_pos[1]) place_pose.position.z = float(dropbox_pos[2]) # TODO: Assign the arm to be used for pick_place arm_name = String() arm_name.data = dropbox_dict.get(object_list_param[i]['group'])[0] # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format # Populate various ROS messages #rospy.loginfo('ROS msg {}, {}, {}, {}, {}'.format(test_scene_num, arm_name, object_name, pick_pose, place_pose) ) #print(object_name.data) yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request #resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose) print("Response: ", resp.success) except rospy.ServiceException, e: print "Service call failed: %s" % e
def pr2_mover(object_list): # TODO: Initialize variables labels = [] centroids = [] dict_list = [] # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_filename = '/home/robond/catkin_ws/src/RoboND-Perception-Project/pr2_robot/config/dropbox.yaml' dropbox_dict = None with open(dropbox_filename, "r") as stream: dropbox_dict = yaml.load(stream) launch_filename = '/home/robond/catkin_ws/src/RoboND-Perception-Project/pr2_robot/launch/pick_place_project.launch' world_name = "test3.world" """ with open(launch_filename, "r") as stream: element = et.parse(stream) nodes = element.findall('./launch/include/arg') for n in nodes: print(n.text) if n.attrib('name') == 'world_name': world_name = n.attrib('value') break """ # TODO: Parse parameters into individual variables print("World Name: " + world_name) test_scene_num = re.findall(r'\d', world_name)[0] # no error handling TEST_SCENE_NUM = Int32() TEST_SCENE_NUM.data = int(test_scene_num) # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list for i in range(0, len(object_list_param)): object_name = object_list_param[i]['name'] object_group = object_list_param[i]['group'] # TODO: Get the PointCloud for a given object and obtain it's centroid do = find_object_by_name(object_list, object_name) points_arr = ros_to_pcl(do.cloud).to_array() centroid = np.mean(points_arr, axis=0)[0:3] centroid_np_array = np.array([ np.asscalar(centroid[0]), np.asscalar(centroid[1]), np.asscalar(centroid[2]) ]) labels.append(do.label) centroids.append(centroid_np_array) OBJECT_NAME = String() OBJECT_NAME.data = object_name PICK_POSE = Pose() PICK_POSE.position.x = np.asscalar(centroid[0]) PICK_POSE.position.y = np.asscalar(centroid[1]) PICK_POSE.position.z = np.asscalar(centroid[2]) # TODO: Create 'place_pose' for the object place_position = dropbox_dict['dropbox'][1][ 'position'] if object_group == 'green' else dropbox_dict[ 'dropbox'][0]['position'] PLACE_POSE = Pose( ) # Assuming green/right always the second element in the array PLACE_POSE.position.x = place_position[0] PLACE_POSE.position.y = place_position[1] PLACE_POSE.position.z = place_position[2] # TODO: Assign the arm to be used for pick_place WHICH_ARM = String() WHICH_ARM.data = 'right' if object_group == 'green' else 'left' # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dict = make_yaml_dict(TEST_SCENE_NUM, WHICH_ARM, OBJECT_NAME, PICK_POSE, PLACE_POSE) dict_list.append(yaml_dict) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) print("Response: ", resp.success) except rospy.ServiceException, e: print "Service call failed: %s" % e
def control(): msg = Int32() msg.data = 16 #pub.publish(msg) return render_template("control.html")
def start_single(self, people_id): self.num_run_pub.publish(Int32(np.mean(np.asarray(self.num_reps)))) self.controller.start_single(people_id)
def pr2_mover(object_list): # calculate centroids for detected objects and store in list labels = [] centroids = [] # to be list of tuples (x, y, z) for object in object_list: labels.append(object.label) points_arr = ros_to_pcl(object.cloud).to_array() centroids.append(np.mean(points_arr, axis=0)[:3]) # TODO: Initialize variables yaml_dict_list = [] test_scene_num = Int32() object_name = String() which_arm = String() pick_pose = Pose() place_pose = Pose() # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') left_place_pose = dropbox_param[0]['position'] right_place_pose = dropbox_param[1]['position'] # TODO: Rotate PR2 in place to capture side tables for the collision map detected_labels = [object.label for object in object_list] # TODO: Loop through the pick list for i in range(len( object_list_param)): #iterate over all objects in the pick list print i if object_list_param[i]['name'] in detected_labels: # print object_list_param[i]['name'] index = detected_labels.index(object_list_param[i]['name']) object_name.data = object_list_param[i]['name'] test_scene_num.data = 3 which_arm.data = object_list_param[i]['group'] if (which_arm.data == 'red'): place_pose.position.x = left_place_pose[0] place_pose.position.y = left_place_pose[1] place_pose.position.z = left_place_pose[2] print 'left' else: place_pose.position.x = right_place_pose[0] place_pose.position.y = right_place_pose[1] place_pose.position.z = right_place_pose[2] print 'right' #print(centroids[index]) pick_pose.position.x = np.asscalar(centroids[index][0]) pick_pose.position.y = np.asscalar(centroids[index][1]) pick_pose.position.z = np.asscalar(centroids[index][2]) # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dict_list.append( make_yaml_dict(test_scene_num, which_arm, object_name, pick_pose, place_pose)) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') print 'service wait completed' try: pick_place_routine = rospy.ServiceProxy( 'pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(test_scene_num, object_name, which_arm, pick_pose, place_pose) print("Response: ", resp.success) except rospy.ServiceException, e: print "Service call failed: %s" % e
if __name__ == "__main__": sample_rate = 5 # Hz rate_pub = rospy.Publisher(topic_set_rate, Int32, queue_size=10) bridge_enable_pub = rospy.Publisher(topic_bridge_enable, String, queue_size=1, latch=True) motors_pub = rospy.Publisher(topic_cmd_vel, Twist, queue_size=10) sonar_sub = rospy.Subscriber(topic_sonars_response, Sonar_data, data_callback) rospy.init_node("obstacle_avoider_simple") rate_pub.publish(Int32(sample_rate)) bridge_enable_msg = String() bridge_enable_msg.data = "run" bridge_enable_pub.publish(bridge_enable_msg) rospy.loginfo("Published enable comd") rate = rospy.Rate(sample_rate) while not rospy.is_shutdown(): direction, dist = get_direction() twist = set_Twist_msg(direction) rospy.loginfo("X: {}, Z: {}".format(twist.linear.x, twist.angular.z)) motors_pub.publish(twist) rate.sleep()
import rospy from std_msgs.msg import Int32 i = Int32() def fibonaci(var): [i, j, k] = [0, 1, 0] while (k < var): k = i + j i = j j = k pub.publish(k) rate.sleep() while not rospy.is_shutdown(): rospy.init_node('node_name') pub = rospy.Publisher("topic", Int32, queue_size=50) rate = rospy.Rate(1) limit = input(" Enter the limit ") fibonaci(limit)
def publish_opcode(self, event=None, op=None): data = Int32(data=op) self.auto_dig_publisher.publish(data)
def callback(msg): doubled = Int32() doubled.data = msg.data * 2 print(doubled.data) pub.publish(doubled)
def pr2_mover(object_list): # TODO: Initialize variables labels = [] centroids = [] obj_label = {} test_scene_num = Int32() test_scene_num.data = 2 dict_list = [] object_name = String() arm_name = String() pick_pose = Pose() place_pose = Pose() # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') # TODO: Parse parameters into individual variable box_left = dropbox_param[0]['position'] box_right = dropbox_param[1]['position'] for object in object_list: labels.append(object.label) print(labels) points_arr = ros_to_pcl(object.cloud).to_array() # TODO: Get the PointCloud for a given object and obtain it's centroid centroid = np.mean(points_arr, axis=0)[:3] centroids.append(centroid) obj_label[object.label] = centroid for i in range(0, len(object_list_param)): object_name.data = object_list_param[i]['name'] object_group = object_list_param[i]['group'] pick_pose.position.x = np.asscalar(obj_label[object_name.data][0]) pick_pose.position.y = np.asscalar(obj_label[object_name.data][1]) pick_pose.position.z = np.asscalar(obj_label[object_name.data][2]) # TODO: Assign the arm to be used for pick_place if object_list_param[i]['group'] == 'red': arm_name.data = 'left' else: arm_name.data = 'right' if arm_name.data == 'left': box_target = box_left else: box_target = box_right place_pose.position.x = box_target[0] place_pose.position.y = box_target[1] place_pose.position.z = box_target[2] yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format ''' # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose) print ("Response: ",resp.success) except rospy.ServiceException, e: print "Service call failed: %s"%e ''' # TODO: Output your request parameters into output yaml file send_to_yaml("output_2.yaml", dict_list) rospy.loginfo("yaml sent")
def pose_cb(self, pose): if self.waypoints is None: rospy.error('No base_waypoints have been received by master') return if not self.dbw_enabled: return self.current_pose = pose # Compute the index of the waypoint closest to the current pose. closest_wp_idx = helper.next_waypoint_index_kdtree( self.current_pose.pose, self.waypoints_kdtree) # Find the closest stop line position if we have to stop the car. _, stop_line_idx = self.stop_line_positions_kdtree.query( [pose.pose.position.x, pose.pose.position.y]) stop_line_positions = self.stop_line_positions[stop_line_idx] # Find the closest waypoint index for the stop line position stop_line_pose = PoseStamped() stop_line_pose.pose.position.x = stop_line_positions[0] stop_line_pose.pose.position.y = stop_line_positions[1] stop_line_pose.pose.orientation = pose.pose.orientation stop_line_waypoint_idx = helper.next_waypoint_index_kdtree( stop_line_pose.pose, self.waypoints_kdtree) self.stop_line_waypoint_pub.publish(Int32(stop_line_waypoint_idx)) if closest_wp_idx == stop_line_waypoint_idx or self.current_velocity == 0.0: rospy.logerr( 'Stop Line waypoint reached. Deceleration Sequence Stopped. Current Vel: %s', self.current_velocity) self.deceleration_started = False self.deceleration_waypoints = None # If the light is RED or YELLOW then slowly decrease the speed. if self.current_traffic_light is not None: rospy.logwarn( 'Light color: %s', helper.get_traffic_light_color( self.current_traffic_light.state)) if self.current_traffic_light.state == 0 or self.current_traffic_light.state == 1: if helper.deceleration_rate( self.current_velocity, self.distance(self.waypoints, closest_wp_idx, stop_line_waypoint_idx)) > 0.1: if not self.deceleration_started: rospy.logwarn('Deceleration Sequence Started') new_waypoints = self.set_velocity_leading_to_stop_point( closest_wp_idx, stop_line_waypoint_idx) self.deceleration_started = True self.deceleration_waypoints = new_waypoints else: rospy.logwarn( 'Using DECELERATION WAYPOINTS CALCULATED BEFORE') new_waypoints = self.deceleration_waypoints elif closest_wp_idx != stop_line_waypoint_idx: # Simulate the behavior of a green light new_waypoints = self.behavior_lights_green(closest_wp_idx) else: rospy.logerr( 'NOT PUBLISHING ANYTHING SINCE ASSUMING CAR IS AT STOP' ) return # Do not publish anything. The car is at a STOP and we don't need to do anything else: # Lights are green new_waypoints = self.behavior_lights_green(closest_wp_idx) else: new_waypoints = self.behavior_lights_green(closest_wp_idx) if stop_line_waypoint_idx - closest_wp_idx < 5: rospy.logerr('Current Waypoint: %s Current Velocity: %s', closest_wp_idx, self.current_velocity) for i in range(stop_line_waypoint_idx - 5, stop_line_waypoint_idx + 1): rospy.loginfo('%s, %s', i, new_waypoints[i].twist.twist.linear.x) # Find number of waypoints ahead dictated by LOOKAHEAD_WPS. However, if the car is stopped then don't accelerate next_wps = new_waypoints[closest_wp_idx + 1:closest_wp_idx + LOOKAHEAD_WPS] self.publish(next_wps)
def pr2_mover(object_list): # TODO: Initialize variables test_scene_num = Int32() arm_name = String() object_name = String() object_group = String() pick_pose = Pose() place_pose = Pose() # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') # TODO: Parse parameters into individual variables object_names = [ object_list_param[i]['name'] for i in range(0, len(object_list_param)) ] object_groups = [ object_list_param[i]['group'] for i in range(0, len(object_list_param)) ] dropbox_names = [ dropbox_param[i]['name'] for i in range(0, len(dropbox_param)) ] dropbox_groups = [ dropbox_param[i]['group'] for i in range(0, len(dropbox_param)) ] dropbox_positions = [ dropbox_param[i]['position'] for i in range(0, len(dropbox_param)) ] # TODO: Rotate PR2 in place to capture side tables for the collision map # TODO: Loop through the pick list for object labels and centroids labels = [] centroids = [] # to be list of tuples (x, y, z) for object in object_list: # TODO: Get the PointCloud for a given object and obtain it's centroid labels.append(object.label) points_arr = ros_to_pcl(object.cloud).to_array() centroids.append(np.mean(points_arr, axis=0)[:3]) # TODO: ensure test_scene_num is correct for the scene being simulated test_scene_num.data = int( 3) # change this to suit the world being simulated # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format dict_list = [] for i in range(0, len(object_list_param)): # Populate various ROS messages object_name.data = str(labels[i]) # TODO: Create 'pick_pose' for the object pick_pose.position.x = np.asscalar(centroids[i][0]) pick_pose.position.y = np.asscalar(centroids[i][1]) pick_pose.position.z = np.asscalar(centroids[i][2]) # TODO: Assign the arm to be used for pick_place if object_groups[i] == 'red': arm_name.data = str('left') else: arm_name.data = str('right') # TODO: Create 'place_pose' for the object place_pose.position.x = dropbox_positions[dropbox_groups.index( object_groups[i])][0] place_pose.position.y = dropbox_positions[dropbox_groups.index( object_groups[i])][1] place_pose.position.z = dropbox_positions[dropbox_groups.index( object_groups[i])][2] yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # TODO: Insert your message variables to be sent as a service request resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose) print("Response: ", resp.success) except rospy.ServiceException, e: print "Service call failed: %s" % e
def pr2_mover(object_list): # Initialize variables test_scene_num = Int32() test_scene_num.data = 3 object_name = String() object_group = String() arm_name = String() pick_pose = Pose() place_pose = Pose() yaml_dict_list = [] # Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') # Parse parameters into individual variables labels = [] centroids = [] # to be list of tuples (x, y, z) for object in object_list: labels.append(object.label) points_arr = ros_to_pcl(object.cloud).to_array() centroids.append(np.mean(points_arr, axis=0)[:3]) #print labels #print centroids # TODO: Rotate PR2 in place to capture side tables for the collision map print len(object_list_param) # Loop through the pick list for i in range(len(object_list_param)): object_name.data = object_list_param[i]['name'] object_group.data = object_list_param[i]['group'] # Get the PointCloud for a given object and obtain it's centroid idx = labels.index(object_name.data) pick_pose.position.x = np.asscalar(centroids[idx][0]) pick_pose.position.y = np.asscalar(centroids[idx][1]) pick_pose.position.z = np.asscalar(centroids[idx][2]) # Create 'place_pose' for the object & # assign the arm to be used for pick_place if (object_group.data == 'green'): position = [0, -0.71, 0.605] arm_name.data = 'right' else: position = [0, 0.71, 0.605] arm_name.data = 'left' place_pose.position.x = position[0] place_pose.position.y = position[1] place_pose.position.z = position[2] # Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) yaml_dict_list.append(yaml_dict) # Wait for 'pick_place_routine' service to come up rospy.wait_for_service('pick_place_routine') try: pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # Insert your message variables to be sent as a service request resp = pick_place_routine(test_scene_num, arm_name, object_name, pick_pose, place_pose) print("Response: ", resp.success) except rospy.ServiceException, e: print "Service call failed: %s" % e
def update(self): for i, pub in enumerate(self.enc_publishers): if pub is None: continue address = self.parameters[i]['address'] if self.parameters[i]['motor_num'] == 1: response = roboclaw.ReadEncM1(address) elif self.parameters[i]['motor_num'] == 2: response = roboclaw.ReadEncM2(address) else: continue msg = Int32() msg.data = int(response[1]) pub.publish(msg) for i, t in enumerate(self.timeouts): if self.enc_publishers[i] is None: continue if int(round(time.time() * 1000)) - t > self.TIMEOUT_TIME: address = self.parameters[i]['address'] if self.parameters[i]['motor_num'] == 1: roboclaw.ForwardBackwardM1(address, 64) elif self.parameters[i]['motor_num'] == 2: roboclaw.ForwardBackwardM2(address, 64) else: continue ''' for i, pwm in enumerate(self.pwms): if pwm is None: continue if self.parameters[i] is not None: address = self.parameters[i]['address'] if self.parameters[i]['motor_num'] == 1: roboclaw.ForwardBackwardM1(address, pwm) elif self.parameters[i]['motor_num'] == 2: roboclaw.ForwardBackwardM2(address, pwm) self.pwms[i] = None ''' for i, vel in enumerate(self.velocities): if vel is None: continue #print('found_vel: ' + str(vel)) if self.parameters[i] is not None: address = self.parameters[i]['address'] #print('address: ' + str(address)) if self.parameters[i]['motor_num'] == 1: #print('setting m1 speed') if roboclaw.SpeedM1(address, vel): #print('m1_set') pass else: #print('m1_failed') pass elif self.parameters[i]['motor_num'] == 2: #print('setting m2 speed') if roboclaw.SpeedM2(address, vel): #print('m1_set') pass else: #print('m1_failed') pass self.velocities[i] = None '''
def RealShoot(self, power, pos) : msg = Int32() msg.data = power self.shoot_pub.publish(msg)
def pr2_mover(object_list): # Initialize variables test_scene_num = Int32() arm_name = String() object_name = String() pick_pose = Pose() place_pose = Pose() dict_list = [] # Get/Read parameters object_list_param = rospy.get_param('/object_list') #print(object_list_param) dropbox_param = rospy.get_param('/dropbox') test_scene_num.data = 2 # Rotate PR2 in place to capture side tables for the collision map #pr2_base_mover_pub.publish(1.57) #print('moving left') #rospy.sleep(5.0) #pr2_base_mover_pub.publish(-1.57) #print('moving right') #rospy.sleep(5.0) #pr2_base_mover_pub.publish(0) #rospy.sleep(5.0) # Parse parameters into individual variables labels = [] centroids = [] for object in object_list: labels.append(object.label) points_arr = ros_to_pcl(object.cloud).to_array() centroids.append(np.mean(points_arr, axis=0)[:3]) #print(object.label, points_arr) #print(object.label) # Loop through the pick list #print(len(object_list_param)) for i in range(0, len(object_list_param)): object_name.data = object_list_param[i]['name'] object_group = object_list_param[i]['group'] # Get the PointCloud for a given object and obtain it's centroid try: index = labels.index(object_name.data) except ValueError: print('Object not detected') continue pick_pose.position.x = np.asscalar(centroids[index][0]) pick_pose.position.y = np.asscalar(centroids[index][1]) #print(index, pick_pose.position.y) pick_pose.position.z = np.asscalar(centroids[index][2]) # Create 'place_pose' for the object for j in range(0, len(dropbox_param)): if object_group == dropbox_param[j]['group']: place_pose.position.x = dropbox_param[j]['position'][0] place_pose.position.y = dropbox_param[j]['position'][1] place_pose.position.z = dropbox_param[j]['position'][2] # Assign the arm to be used for pick_place if object_group == 'green': arm_name.data = 'right' elif object_group == 'red': arm_name.data = 'left' # Create a list of dictionaries yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) #print(yaml_dict) print(object_name, pick_pose.position.y) dict_list.append(yaml_dict) # Place item (disabled) # rospy.wait_for_service('pick_place_routine') # try: # pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # Message variables to be sent as a service request # resp = pick_place_routine(test_scene_num,object_name,arm_name,pick_pose,place_pose) # print ("Response: ",resp.success) # except rospy.ServiceException, e: # print "Service failed: %s"%e filename = 'output_' + str(test_scene_num.data) + '.yaml' send_to_yaml(filename, dict_list) return
def pcl_callback(pcl_msg): # Exercise-2 TODOs: # TODO: Convert ROS msg to PCL data cloud = ros_to_pcl(pcl_msg) # TODO: Statistical Outlier Filtering outlier_filter = cloud.make_statistical_outlier_filter() outlier_filter.set_mean_k(100) outlier_filter.set_std_dev_mul_thresh(0.003) cloud_filtered = outlier_filter.filter() # pcl.save(cloud_filtered, "project_table_scene.pcd") # # TODO: Voxel Grid Downsampling vox = cloud_filtered.make_voxel_grid_filter() LEAF_SIZE = 0.002 vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE) cloud_filtered = vox.filter() # pcl.save(cloud_filtered, "voxel_downsampled.pcd") # # TODO: PassThrough Filter passthrough_z= cloud_filtered.make_passthrough_filter() passthrough_z.set_filter_field_name("z") passthrough_z.set_filter_limits(0.62, 1.2) cloud_filtered_z = passthrough_z.filter() passthrough_y= cloud_filtered_z.make_passthrough_filter() passthrough_y.set_filter_field_name("y") passthrough_y.set_filter_limits(-0.50, 0.3) cloud_filtered = passthrough_y.filter() # pcl.save(cloud_filtered, "pass_through_filtered.pcd") # # TODO: RANSAC Plane Segmentation seg = cloud_filtered.make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) max_distance = 0.08 seg.set_distance_threshold(max_distance) inliers, coefficients = seg.segment() # # TODO: Extract inliers and outliers cloud_objects = cloud_filtered.extract(inliers, negative=False) # pcl.save(cloud_objects, "extracted_inliers.pcd") # # Extract outliers cloud_table = cloud_filtered.extract(inliers, negative=True) # pcl.save(cloud_table, "extracted_outliers.pcd") # # # TODO: Euclidean Clustering white_cloud = XYZRGB_to_XYZ(cloud_objects) tree = white_cloud.make_kdtree() # TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately ec = white_cloud.make_EuclideanClusterExtraction() ec.set_ClusterTolerance(0.02) ec.set_MinClusterSize(300) ec.set_MaxClusterSize(10000) # Search the k-d tree for clusters ec.set_SearchMethod(tree) # Extract indices for each of the discovered clusters cluster_indices = ec.Extract() #Assign a color corresponding to each segmented object in scene cluster_color = get_color_list(len(cluster_indices)) color_cluster_point_list = [] for j, indices in enumerate(cluster_indices): for i, indice in enumerate(indices): color_cluster_point_list.append([white_cloud[indice][0], white_cloud[indice][1], white_cloud[indice][2], rgb_to_float(cluster_color[j])]) #Create new cloud containing all clusters, each with unique color cluster_cloud = pcl.PointCloud_PointXYZRGB() cluster_cloud.from_list(color_cluster_point_list) # pcl.save(cluster_cloud, "cluster_cloud.pcd") # TODO: Convert PCL data to ROS messages ros_cloud_objects = pcl_to_ros(cloud_objects) ros_cloud_table = pcl_to_ros(cloud_table) ros_cluster_cloud = pcl_to_ros(cluster_cloud) # TODO: Publish ROS messages pcl_objects_pub.publish(ros_cloud_objects) pcl_table_pub.publish(ros_cloud_table) pcl_cluster_pub.publish(ros_cluster_cloud) # Exercise-3 TODOs: detected_objects_labels = [] detected_objects = [] for index, pts_list in enumerate(cluster_indices): # Grab the points for the cluster from the extracted outliers (cloud_objects) pcl_cluster = cloud_objects.extract(pts_list) # TODO: convert the cluster from pcl to ROS using helper function ros_cluster = pcl_to_ros(pcl_cluster) # Extract histogram features # TODO: complete this step just as is covered in capture_features.py chists = compute_color_histograms(ros_cluster, using_hsv=True) normals = get_normals(ros_cluster) nhists = compute_normal_histograms(normals) feature = np.concatenate((chists, nhists)) # hist_bins=64 # chists = compute_color_histograms(ros_cluster, nbins=hist_bins, using_hsv=True) # normals = get_normals(ros_cluster) # nhists = compute_normal_histograms(normals, nbins=hist_bins) # feature = np.concatenate((chists, nhists)) # Make the prediction, retrieve the label for the result # and add it to detected_objects_labels list prediction = clf.predict(scaler.transform(feature.reshape(1,-1))) label = encoder.inverse_transform(prediction)[0] detected_objects_labels.append(label) # Publish a label into RViz label_pos = list(white_cloud[pts_list[0]]) label_pos[2] += .4 object_markers_pub.publish(make_label(label,label_pos, index)) # Add the detected object to the list of detected objects. do = DetectedObject() do.label = label do.cloud = ros_cluster detected_objects.append(do) rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels)) # Publish the list of detected objects # This is the output you'll need to complete the upcoming project! detected_objects_pub.publish(detected_objects) # detected_objects_pub.publish(pcl_msg) # Suggested location for where to invoke your pr2_mover() function within pcl_callback() # Could add some logic to determine whether or not your object detections are robust # before calling pr2_mover() # try: # pr2_mover(detected_objects_list) # except rospy.ROSInterruptException: # pass # function to load parameters and request PickPlace service # def pr2_mover(object_list): # TODO: Initialize variables test_scene_num = Int32() object_name = String() arm_name = String() pick_pose = Pose() place_pose = Pose() dict_list = [] yaml_filename = 'output_1.yaml' test_scene_num.data = 1 labels = [] centroids = [] for objects in detected_objects: labels.append(objects.label) points_arr = ros_to_pcl(objects.cloud).to_array() centroids.append(np.mean(points_arr, axis=0)[:3]) # # TODO: Get/Read parameters object_list_param = rospy.get_param('/object_list') dropbox_param = rospy.get_param('/dropbox') # # TODO: Rotate PR2 in place to capture side tables for the collision map # # TODO: Loop through the pick list # # TODO: Parse parameters into individual variables for i in range(0, len(object_list_param)): object_name.data = object_list_param[i]['name'] object_group = object_list_param[i]['group'] # # TODO: Get the PointCloud for a given object and obtain it's centroid for j in range(0, len(labels)): if object_name.data == labels[j]: pick_pose.position.x = np.asscalar(centroids[j][0]) pick_pose.position.y = np.asscalar(centroids[j][1]) pick_pose.position.z = np.asscalar(centroids[j][2]) # # TODO: Create 'place_pose' for the object for j in range(0, len(dropbox_param)): if object_group == dropbox_param[j]['group']: place_pose.position.x = dropbox_param[j]['position'][0] place_pose.position.y = dropbox_param[j]['position'][1] place_pose.position.z = dropbox_param[j]['position'][2] # # TODO: Assign the arm to be used for pick_place if object_group == 'green': arm_name.data = 'right' elif object_group == 'red': arm_name.data = 'left' # # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose) dict_list.append(yaml_dict) # # Wait for 'pick_place_routine' service to come up # rospy.wait_for_service('pick_place_routine') # try: # pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace) # # TODO: Insert your message variables to be sent as a service request # resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE) # print ("Response: ",resp.success) # except rospy.ServiceException, e: # print "Service call failed: %s"%e # # TODO: Output your request parameters into output yaml file send_to_yaml(yaml_filename, dict_list)
def publish_mission_state(mission_state): global mission_state_pub mission_state_msg = Int32() mission_state_msg.data = mission_state mission_state_pub.publish(mission_state_msg)