def required_human_cb(self, tau): msg = Float32MultiArray() tau = self.calculate_dynamics(tau.effort) if self.enable_control: msg.data = tau - self.tau self.torque_error_pub.publish(msg)
def main(): global leftSpeed global rightSpeed global panPos global tiltPos global speedX global speedY global yaw global stop global mov_spine global spine global start leftSpeed = 0 rightSpeed = 0 panPos = 0 tiltPos = 0 speedY = 0 speedX = 0 yaw = 0 spine = 0 mov_spine=False start = 0 msgSpeeds = Float32MultiArray() msgHeadPos = Float32MultiArray() msgSpine = Float32() msgTwist = Twist() msgStart = Bool() print "" print "---------------------------->" print "INITIALIZING JOYSTICK TELEOP BY [EDD-II]" rospy.init_node("joystick_teleop") rospy.Subscriber("/hardware/joy", Joy, callbackJoy) #rospy.Subscriber("/joy", Joy, callbackJoy) ## TODO: VERIFY--- topics names to head and spine pubSpin = rospy.Publisher("/hardware/torso/goal_spine", Float32, queue_size=1) pubTwist = rospy.Publisher("/hsrb/command_velocity", Twist, queue_size =1) pubHeadPos = rospy.Publisher("/hardware/head/goal_pose", Float32MultiArray, queue_size=1) pubStart = rospy.Publisher("/hardware/start_button", Bool, queue_size=1) loop = rospy.Rate(10) while not rospy.is_shutdown(): if math.fabs(speedX) > 0 or math.fabs(speedY) > 0 or math.fabs(yaw) > 0: msgTwist.linear.x = speedX msgTwist.linear.y = speedY/2.0 msgTwist.linear.z = 0 msgTwist.angular.z = yaw pubTwist.publish(msgTwist) if spine <= 0.51 and spine >= -0.51 and mov_spine==True: if(spine_button == 1 and spine < 0.5 ): spine=spine+0.01 if(spine_button ==-1 and spine > -0.5): spine=spine-0.01 msgSpine.data = spine pubSpin.publish(msgSpine) if math.fabs(panPos) > 0.1 or math.fabs(tiltPos) > 0.1 : msgHeadPos.data = [panPos, tiltPos] pubHeadPos.publish(msgHeadPos) if start: msgStart = True pubStart.publish(msgStart) rospy.sleep(2.0) msgStart = False pubStart.publish(msgStart) loop.sleep()
txdata.append(check) servo.write(txdata) time.sleep(0.00025) # setup kdl filename = "../urdf/simple_arm.urdf" (ok, tree) = kdl_parser_py.urdf.treeFromFile(filename) chain = tree.getChain("base", "link7") t = kdl.Tree(tree) fksolverpos = kdl.ChainFkSolverPos_recursive(chain) iksolvervel = kdl.ChainIkSolverVel_pinv(chain) iksolverpos = kdl.ChainIkSolverPos_NR(chain, fksolverpos, iksolvervel) pub = rospy.Publisher('servo_ang', Float32MultiArray, queue_size=10) msg = Float32MultiArray() def set_angle(stts, spd, x, y, z): init_ang = 0 init_jnt = kdl.JntArray(6) init_jnt[0] = 0 init_jnt[1] = -math.pi / 2 init_jnt[2] = math.pi / 2 init_jnt[3] = math.pi / 2 init_jnt[4] = -math.pi / 2 init_jnt[5] = 0 q = tf.transformations.quaternion_from_euler(0, 0, 0) F2 = kdl.Frame(kdl.Rotation.Quaternion(q[0], q[1], q[2], q[3]), kdl.Vector(x, y, z))
def pub_turning(self,x,z): array = Float32MultiArray(data=[x,z]) self.turning_publish.publish(array)
def part_2(args): global g_dest_coordinates global g_src_coordinates global g_WORLD_MAP global g_MAP_SIZE_X global g_MAP_SIZE_Y global g_MAP_RESOLUTION_X global g_MAP_RESOLUTION_Y global g_NUM_X_CELLS global g_NUM_Y_CELLS global pose2d_sparki_odometry global isStarting, originPose g_src_coordinates = (float(args.src_coordinates[0]), float(args.src_coordinates[1])) g_dest_coordinates = (float(args.dest_coordinates[0]), float(args.dest_coordinates[1])) # pixel_grid has intensity values for all the pixels # You will have to convert it to the earlier 0 and 1 matrix yourself g_MAP_SIZE_X = 1.8 # 1.8 meters g_MAP_SIZE_Y = 1.2 # 1.2 meters g_MAP_RESOLUTION_X = 0.0015 # Each col represents 10cm g_MAP_RESOLUTION_Y = 0.0015 # Each row represents 10cm pixel_grid = _load_img_to_intensity_matrix(args.obstacles) g_NUM_X_CELLS = 1200 # Number of columns in the grid map g_NUM_Y_CELLS = 800 g_WORLD_MAP = [0] * g_NUM_Y_CELLS * g_NUM_X_CELLS # Convert from meters to ij coordinates using given function source = xy_coordinates_to_ij_coordinates(g_src_coordinates[0], g_src_coordinates[1]) dest = xy_coordinates_to_ij_coordinates(g_dest_coordinates[0], g_dest_coordinates[1]) # Run Dijkstra's on our randomly generated map and our starting source node obstacleMap = [] for n in pixel_grid: for m in n: if m > 0: obstacleMap.append(1) else: obstacleMap.append(0) g_WORLD_MAP = obstacleMap prevArray = run_dijkstra(ij_to_vertex_index(source[0], source[1]), obstacleMap) # Reconstruct the path from our random source to our random destination # Path takes in prevArray from dijkstra's, takes in vertex indices, returns list of vertex indices path = reconstruct_path(prevArray, ij_to_vertex_index(source[0], source[1]), ij_to_vertex_index(dest[0], dest[1])) waypoints = path_into_waypoints(path) print() # Convert path coordinates to image coordinates imageCoordinatesOfPath = [] for step in path: # Convert each path vertex index to i,j coords and then world coordinates to correspond with our image i, j = vertex_index_to_ij(step) x, y = ij_coordinates_to_xy_coordinates(i, j) # Convert new world coordinates to image coordinates by figuring out where that world coordinate is on our map image # round((meter position of point / total meters) * total cells) = proportion of image/map for that x/y (assuming equal size) x2 = ceil((x / g_MAP_SIZE_X) * g_NUM_X_CELLS) y2 = g_NUM_Y_CELLS - ceil((y / g_MAP_SIZE_Y) * g_NUM_Y_CELLS) imageCoordinatesOfPath.append((x2, y2)) # Draw our path img = Image.open(args.obstacles) draw = ImageDraw.Draw(img) print(f"(x, y) Image Coordinates of Source: {imageCoordinatesOfPath[-1]}") print( f"(x, y) Image Coordinates of Destination: {imageCoordinatesOfPath[0]}" ) start = imageCoordinatesOfPath[0] for n in range(len(imageCoordinatesOfPath) - 1): line = [] line.append(start) line.append(imageCoordinatesOfPath[n]) draw.line(line, fill=255, width=3) start = imageCoordinatesOfPath[n + 1] img.save('answerD.jpg') #DRIVE initSimulator(args) # TODID: Set up your publishers and subscribers rate = rospy.Rate(CYCLE_HZ) currentWaypointIndex = 1 waypointCount = len(waypoints) angleTolerance = .09 distTolerance = .001 while currentWaypointIndex < waypointCount: targetWaypoint = waypoints[currentWaypointIndex] motorSpeeds = Float32MultiArray() #spin to find bearing targetTheta = bearingError(pose2d_sparki_odometry, targetWaypoint) % (2 * 3.14159) adjCurrentTheta = pose2d_sparki_odometry.theta % (2 * 3.14159) print("TARGET THETA: ", targetTheta) print(pose2d_sparki_odometry.x, pose2d_sparki_odometry.y) print(targetWaypoint[0], targetWaypoint[1]) while abs(adjCurrentTheta - targetTheta) > angleTolerance: print(adjCurrentTheta) scalar = max( abs(adjCurrentTheta - targetTheta) / (2 * 3.14159), .5) #if(adjCurrentTheta > targetTheta): # motorSpeeds.data = [-1 * SPARKI_VELOCITY * scalar, SPARKI_VELOCITY * scalar] #elif(adjCurrentTheta < targetTheta): motorSpeeds.data = [ SPARKI_VELOCITY * scalar, -1 * SPARKI_VELOCITY * scalar ] publisher_motor.publish(motorSpeeds) publisher_render.publish() adjCurrentTheta = pose2d_sparki_odometry.theta % (2 * 3.14159) rate.sleep() #stop when bearing acheived motorSpeeds.data = [0.0, 0.0] publisher_motor.publish(motorSpeeds) publisher_render.publish() rate.sleep() #drive forwward to target targetDist = posError(pose2d_sparki_odometry, targetWaypoint) lastTargetDist = 9999999999 while targetDist > distTolerance: if (targetDist > lastTargetDist): break motorSpeeds.data = [SPARKI_VELOCITY, SPARKI_VELOCITY] publisher_motor.publish(motorSpeeds) publisher_render.publish() lastTargetDist = targetDist targetDist = posError(pose2d_sparki_odometry, targetWaypoint) rate.sleep() #stop at waypoint motorSpeeds.data = [0.0, 0.0] publisher_motor.publish(motorSpeeds) publisher_render.publish() #advance to next point currentWaypointIndex = currentWaypointIndex + 1 rate.sleep() print(pose2d_sparki_odometry.x, pose2d_sparki_odometry.y) print(targetWaypoint[0], targetWaypoint[1])
def vanilla(args): # Instantiate the agent with the tf model model = Agent(PATH_MODEL[args.robot][args.method]) print("started") # if args.robot == 1: pub = rospy.Publisher("/usb2servo/joint_angles", Float64MultiArray, queue_size=1) else: pub = rospy.Publisher("joint_angles", Float32MultiArray, queue_size=1) # ROS nodes init sub = rospy.Subscriber("observations", Obs, subscriber_callback, queue_size=1) rospy.init_node('vanilla') rate = rospy.Rate(10) array = Float64MultiArray() if args.robot == 1 else Float32MultiArray() fifo_obs = Queue.Queue() # init log buffer buffer_observations = [] buffer_delayed_observations = [] buffer_unn_instructions = [] buffer_effec_pose = [] nb_step = 0 test_length = CHANGE_FREQ * len( TEST_VEC) if args.test == 0 else CHANGE_FREQ * len( DESIRED_BALL_POSITION) print("started") while not rospy.is_shutdown(): if received_obs and observations is not None: # get observations (delayed by ~ 300 ms) obs = get_observations(nb_step / CHANGE_FREQ, args) # put the newly obtained observation into a buffer fifo_obs.put(obs) # if the buffer of size ADDED_DELAY +1 is full if fifo_obs.qsize() == ADDED_DELAY + 1: # we get the delayed obs # if ADDED_DELAY = 0 we always get the newly added obs delayed_obs = fifo_obs.get() buffer_observations.append(obs) buffer_delayed_observations.append(delayed_obs) if delayed_obs is not None: # feed the model with the observations to get the joints offsets actions = model.act(delayed_obs) # scale and clip if args.robot == 0: actions = np.clip(actions, -1, 1) actions[0] = np.clip( actions[0] * 5 + REF_POSITION_JOINTS[args.robot][0], 0, 180) actions[1] = np.clip( actions[1] * 15 + REF_POSITION_JOINTS[args.robot][1], 0, 180) actions[2] = np.clip( actions[2] * 15 + REF_POSITION_JOINTS[args.robot][2], 0, 180) command = np.clip([ 60, actions[0] - 2, actions[1] + 6, actions[2] - 5, 90, 30 ], 0, 180) else: actions[0] = np.clip( actions[0], -0.5, 0.5) * 86 + REF_POSITION_JOINTS[args.robot][0] actions[1] = np.clip( actions[1], -1, 0.2) * -73 + REF_POSITION_JOINTS[args.robot][1] command = np.clip([ 100, actions[0] + 3, actions[0] + 3, actions[1] + 27, 90, 25 ], 0, 180) command = np.rint(command) #publish_action(pub,array,command) nb_step += 1 tmp = list(obs) if args.method == 0: del tmp[1] print_step(obs, command, nb_step) if len(buffer_observations ) == test_length - ADDED_DELAY and args.log is not None: save_log(buffer_delayed_observations, buffer_effec_pose, buffer_unn_instructions, "2DoFVanillaDA", "logs") score = computeError(np.array(buffer_observations)) print("##### Perf pour le delay {} : {} #####".format( DELAY, score / 50)) break rate.sleep()
def publish_motors(self): msg = Float32MultiArray() msg.data = self.controller.power self.motor_pub.publish(msg)
__author__ = "Gabriel Urbain" __copyright__ = "Copyright 2017, Human Brain Projet, SP10" __license__ = "MIT" __version__ = "1.0" __maintainer__ = "Gabriel Urbain" __email__ = "*****@*****.**" __status__ = "Research" __date__ = "September 12th, 2017" # Create node rospy.init_node('test_tigrillo', anonymous=True) pub = rospy.Publisher('tigrillo/legs_cmd', Float32MultiArray, queue_size=1) # In a loop, send a actuation signal t = 0 while True: val = [-math.pi/8 * math.sin(0.01*t), math.pi/8 * math.sin(0.01*t), math.pi/8 * math.sin(0.01*t), -math.pi/8 * math.sin(0.01*t)] rospy.loginfo("Publishing in tigrillo/legs_cmd topic: " + str(val)) pub.publish(Float32MultiArray(layout=MultiArrayLayout([], 1), data=val)) time.sleep(0.001) t += 1 if t > 20000: exit()
def Publishfun(self): #创建节点 rospy.init_node('RMS_UI', anonymous=True) #创建一个flag self.flag_pub = rospy.Publisher('/flag', Int8, queue_size=10) #Realposition订阅者创建 rospy.Subscriber("/position_real", Float32MultiArray, self.Position_show) if self.checkBox_1.isChecked(): self.flag = 1 self.flag_pub.publish(self.flag) #positionkeeping发布者创建 self.positionkeeping_pub = rospy.Publisher('/set_point', Float32MultiArray, queue_size=10) setpoint = millerToXY(float(self.PositionKeeping_Lon.text()), float(self.PositionKeeping_Lat.text())) setpoint_1 = [-setpoint[1], setpoint[0]] setpoint_xy = Float32MultiArray(data=setpoint_1) rospy.loginfo(setpoint_xy.data) self.positionkeeping_pub.publish(setpoint_xy) self.textEdit.setText( "The Positionkeeping point is set successfully") elif self.checkBox_2.isChecked(): self.flag = 2 self.flag_pub.publish(self.flag) #pathfollowing发布者创建 self.pathfollowing_pub = rospy.Publisher('/waypoints', pf, queue_size=10) Point1_xy = millerToXY(float(self.Point1_Lon.text()), float(self.Point1_Lat.text())) Point2_xy = millerToXY(float(self.Point2_Lon.text()), float(self.Point2_Lat.text())) Point3_xy = millerToXY(float(self.Point3_Lon.text()), float(self.Point3_Lat.text())) Point4_xy = millerToXY(float(self.Point4_Lon.text()), float(self.Point4_Lat.text())) Point5_xy = millerToXY(float(self.Point5_Lon.text()), float(self.Point5_Lat.text())) pfpoint1 = [-Point1_xy[1], Point1_xy[0]] pfpoint2 = [-Point2_xy[1], Point2_xy[0]] pfpoint3 = [-Point3_xy[1], Point3_xy[0]] pfpoint4 = [-Point4_xy[1], Point4_xy[0]] pfpoint5 = [-Point5_xy[1], Point5_xy[0]] waypoints_xy = pf() #waypoints_xy.data = [pfpoint1,pfpoint2,pfpoint3,pfpoint4,pfpoint5] waypoints_xy.p_1 = pfpoint1 waypoints_xy.p_2 = pfpoint2 waypoints_xy.p_3 = pfpoint3 waypoints_xy.p_4 = pfpoint4 waypoints_xy.p_5 = pfpoint5 self.pathfollowing_pub.publish(waypoints_xy) rospy.loginfo("path following! the way_points are: %s %s %s %s %s", waypoints_xy.p_1, waypoints_xy.p_2, waypoints_xy.p_3, waypoints_xy.p_4, waypoints_xy.p_5) self.textEdit.setText("The pointsway is set successfully") else: pass
def main(): global posicionFinalCar, velocidadM1, velocidadM2, callTime, callPos, pasoRuta #Obtencion del vector de posicion final del robot. Si no se envia se toma por defecto [40,40,pi/2] if len(sys.argv) > 1: posicionFinalCar = [ float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]) ] #Iniciacion del nodo antes ROS rospy.init_node('P2e_RRT', anonymous=False) #Publicacion al topico de velocidades para los motores del robot pioneer pubMotorsVel = rospy.Publisher('motorsVel', Float32MultiArray, queue_size=10) #Suscripcion al topico pioneerPosition rospy.Subscriber("pioneerPosition", Twist, callbackPioneerPosition) #Suscripcion al topico simulationTime rospy.Subscriber("simulationTime", Float32, callbackSimulationTime) #Suscripcion al topico obstaclesPosition rospy.Subscriber('obstaclesPosition', Float32MultiArray, callbackObstaclesPosition) #Tiempo durante el cual duerme el nodo rate = rospy.Rate(20) pasoRuta = 0 try: #Mientras el nodo este activo, se realizara: while not rospy.is_shutdown(): if callTime and callPos and len(rutaObjetivo) != 0: posicionFinal = [0, 0] if pasoRuta == len(rutaObjetivo) - 1: posicionFinal = [ float(rutaObjetivo[pasoRuta].split(",")[0]), float(rutaObjetivo[pasoRuta].split(",")[1]), posicionFinalCar[2] ] else: posicionFinal = [ float(rutaObjetivo[pasoRuta].split(",")[0]), float(rutaObjetivo[pasoRuta].split(",")[1]), 0.0 ] calcularCinematicaRobot(posicionFinal) callTime = False callPos = False #Publica la velocidad del robot en el topico motorsVel mensaje = Float32MultiArray(data=[velocidadM1, velocidadM2]) pubMotorsVel.publish(mensaje) #Se envia a dormir al nodo rate.sleep() except Exception as e: raise e
import rospy import time from std_msgs.msg import Float32MultiArray, Float32 from beginner_tutorials.msg import coordinates from InverseKinmatics import joints_angles DEG2RAD = math.pi / 180 POS2RAD = (2 * math.pi) / 4096 RAD2POS = 4096 / (2 * math.pi) ZERO_OFFSET = 4096 / 2 joints_angles_rad = [0, 0, 0, 0, 0] l4 = 30.25 delta = 1 temp = [0, 0, 0, 0, 0] angles_msg = Float32MultiArray() #location = Float32MultiArray() location = coordinates() pub_location = rospy.Publisher("current_coordinates", coordinates, queue_size=10) def joints_angle_read(data): global POS2RAD global ZERO_OFFSET global joints_angles_rad global temp temp = data.data joints_angles_rad = list(data.data)
def main(screen): pub = rospy.Publisher('motor_control', Float32MultiArray, queue_size=10) rospy.init_node('manual_motor_control', anonymous=True) rate = rospy.Rate(60) motors = [0.0,0.0,0.0,0.0] rows,colums = screen.getmaxyx() screen.nodelay(1) #if no input, just return did = "" while 1: c = screen.getch() if c != -1: screen.erase() if c == ord('Q'): raise("Quit!") if c == 259: #up arrow motors[1] += .025 motors[2] += .025 did = "more thrust forward" if c == 258: #down arrow motors[1] -= .025 motors[2] -= .025 did = "less thrust forward" if c == ord('y'): motors[0] += .025 motors[3] += .025 did = "more lift" if c == ord('h'): motors[0] -= .025 motors[3] -= .025 did = "less lift" if c == ord('!'): motors[0] += .025 did = "motor 0 up" if c == ord('1'): motors[0] -= .025 did = "motor 0 down" if c == ord('@'): motors[1] += .025 did = "motor 1 up" if c == ord('2'): motors[1] -= .025 did = "motor 1 down" if c == ord('#'): motors[2] += .025 did = "motor 2 up" if c == ord('3'): motors[2] -= .025 did = "motor 2 down" if c == ord('$'): motors[3] += .025 did = "motor 3 up" if c == ord('4'): motors[3] -= .025 did = "motor 3 down" if c == ord('~'): motors[0] += .025 motors[1] += .025 motors[2] += .025 motors[3] += .025 did = "all up" if c == ord('`'): motors[0] -= .025 motors[1] -= .025 motors[2] -= .025 motors[3] -= .025 did = "all down" screen.addstr(0,0,"Key: %d"%(c)) screen.addstr(2,0,"did: %s"%(did)) toPub=Float32MultiArray() toPub.data = motors for i in range(len(motors)): if motors[i] > 1: motors[i] = 1 if motors[i] < 0: motors[i] = 0 screen.addstr(1,0,"Motors: b:%f r:%f l:%f f:%f"%( motors[0]*100, motors[1]*100, motors[2]*100, motors[3]*100 )) pub.publish(toPub) rate.sleep()
def depth_callback(self, depth_message): if self.get_flag: print("into ggcnn!") with TimeIt('Crop'): depth = self.bridge.imgmsg_to_cv2(depth_message) # mask=cv2.inRange(depth,0,0.99)/255 # nums = np.sum(mask) # depth = depth*mask # depth[mask==0]=np.max(depth) # Crop a square out of the middle of the depth and resize it to 300*300 crop_size = 400 depth_crop = cv2.resize( depth[(480 - crop_size) // 2:(480 - crop_size) // 2 + crop_size, (640 - crop_size) // 2:(640 - crop_size) // 2 + crop_size], (300, 300)) # mask = cv2.inRange(depth_crop,0,5)/255 # depth_crop = depth_crop*mask # Replace nan with 0 for inpainting. depth_crop = depth_crop.copy() depth_nan = np.isnan(depth_crop).copy() depth_crop[depth_nan] = 0 with TimeIt('Inpaint'): # open cv inpainting does weird things at the border. depth_crop = cv2.copyMakeBorder(depth_crop, 1, 1, 1, 1, cv2.BORDER_DEFAULT) mask = (depth_crop == 0).astype(np.uint8) # Scale to keep as float, but has to be in bounds -1:1 to keep opencv happy. depth_scale = np.abs(depth_crop).max() depth_crop = depth_crop.astype( np.float32 ) / depth_scale # Has to be float32, 64 not supported. depth_crop = cv2.inpaint(depth_crop, mask, 1, cv2.INPAINT_NS) # Back to original size and value range. depth_crop = depth_crop[1:-1, 1:-1] depth_crop = depth_crop * depth_scale with TimeIt('Calculate Depth'): # Figure out roughly the depth in mm of the part between the grippers for collision avoidance. depth_center = depth_crop[ 100:141, 130:171].flatten() #change to 1 dimension depth_center.sort() #sort by row,small to big depth_center = depth_center[:10].mean() * 1000.0 with TimeIt('Inference'): # Run it through the network. depth_crop = np.clip((depth_crop - depth_crop.mean()), -1, 1) depth_crop1 = self.numpy_to_torch(depth_crop) depth_crop_cuda = depth_crop1.to(self.device) pred_out = self.model(depth_crop_cuda) points_out = pred_out[0].squeeze().cpu().detach().numpy( ) #维度为1则降一维 points_out[depth_nan] = 0 with TimeIt('Trig'): # Calculate the angle map. cos_out = pred_out[1].squeeze().cpu().detach().numpy() sin_out = pred_out[2].squeeze().cpu().detach().numpy() ang_out = np.arctan2(sin_out, cos_out) / 2.0 width_out = pred_out[3].squeeze().cpu().detach().numpy( ) * 150.0 # Scaled 0-150:0-1 with TimeIt('Filter'): # Filter the outputs. points_out = ndimage.filters.gaussian_filter( points_out, 5.0) # cnn to filter ang_out = ndimage.filters.gaussian_filter(ang_out, 2.0) with TimeIt('Control'): # Calculate the best pose from the camera intrinsics. maxes = None ALWAYS_MAX = False # Use ALWAYS_MAX = True for the open-loop solution. if self.ROBOT_Z > 0.34 or ALWAYS_MAX: # > 0.34 initialises the max tracking when the robot is reset. # Track the global max. max_pixel = np.array( np.unravel_index(np.argmax(points_out), points_out.shape)) prev_mp = max_pixel.astype(np.int) else: # Calculate a set of local maxes. Choose the one that is closes to the previous one. maxes = peak_local_max(points_out, min_distance=10, threshold_abs=0.1, num_peaks=3) if maxes.shape[0] == 0: self.get_flag = False print("empty") return max_pixel = maxes[np.argmin( np.linalg.norm( maxes - self.prev_mp, axis=1))] #get the point near prev point # Keep a global copy for next iteration. prev_mp = (max_pixel * 0.25 + self.prev_mp * 0.75).astype( np.int) max_pixel1 = max_pixel ang = ang_out[max_pixel[0], max_pixel[1]] width = width_out[max_pixel[0], max_pixel[1]] # Convert max_pixel back to uncropped/resized image coordinates in order to do the camera transform. max_pixel = ((np.array(max_pixel) / 300.0 * crop_size) + np.array([(480 - crop_size) // 2, (640 - crop_size) // 2])) max_pixel = np.round(max_pixel).astype(np.int) point_depth = depth[max_pixel[0], max_pixel[1]] # These magic numbers are my camera intrinsic parameters. x = (max_pixel[1] - self.cx) / (self.fx) * point_depth y = (max_pixel[0] - self.cy) / (self.fy) * point_depth z = point_depth self.pos = [x, y, z, ang, width, depth_center] if np.isnan(z): return if self.pub: with TimeIt('Draw'): # Draw grasp markers on the points_out and publish it. (for visualisation) grasp_img = np.zeros((300, 300, 3), dtype=np.uint8) grasp_img[:, :, 2] = (points_out * 255.0) grasp_img_plain = grasp_img.copy() rr, cc = circle(max_pixel1[0], max_pixel1[1], 5) grasp_img[rr, cc, 0] = 0 grasp_img[rr, cc, 1] = 255 grasp_img[rr, cc, 2] = 0 #depth[rr,cc] = 255 #cv2.imshow("x", depth) with TimeIt('Publish'): # Publish the output images (not used for control, only visualisation) grasp_img = self.bridge.cv2_to_imgmsg(grasp_img, 'bgr8') grasp_img.header = depth_message.header self.grasp_pub.publish(grasp_img) grasp_img_plain = self.bridge.cv2_to_imgmsg( grasp_img_plain, 'bgr8') grasp_img_plain.header = depth_message.header self.grasp_plain_pub.publish(grasp_img_plain) self.depth_pub.publish( self.bridge.cv2_to_imgmsg(depth_crop)) self.ang_pub.publish(self.bridge.cv2_to_imgmsg(ang_out)) self.width_pub.publish( self.bridge.cv2_to_imgmsg(width_out)) # Output the best grasp pose relative to camera. cmd_msg = Float32MultiArray() cmd_msg.data = [x, y, z, ang, width, depth_center] print(cmd_msg.data) self.cmd_pub.publish(cmd_msg) else: self.get_flag = False
def __init__(self): rospy.init_node('runDDPG', anonymous=True) agent = DDPGAgent(state_size=self.n_inputs, action_size=self.n_outputs, goal_size=2, actor_learning_rate=1e-4, critic_learning_rate=3e-4, tau=0.1, load_saved_net=self.load_saved_net) rospy.Subscriber('/RL/gripper_status', String, self.callbackGripperStatus) rospy.Service('/RL/start_learning', Empty, self.start_learning) obs_srv = rospy.ServiceProxy('/RL/observation', observation) drop_srv = rospy.ServiceProxy('/RL/IsObjDropped', IsDropped) move_srv = rospy.ServiceProxy('/RL/MoveGripper', TargetAngles) reset_srv = rospy.ServiceProxy('/RL/ResetGripper', Empty) pub_goal = rospy.Publisher('/RL/Goal', Float32MultiArray, queue_size=10) gg = Float32MultiArray() epochs_count = 0 total_step = 0 total_episodes = 0 visited_states = np.array([[0., 0.]]) reached_goals = np.array([[0., 0.]]) failed_goals = np.array([[0., 0.]]) action = np.array([0., 0.]) rate = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): if self.stLearning: ## Start episode ## epochs_count += 1 successes = 0 ep_total_r = 0 for n in range(self.num_episodes): total_episodes += 1 # Reset gripper reset_srv() while not self.gripper_closed: rate.sleep() # Get observation state = np.array(obs_srv().state) # Sample goal goal = self.sample_goal() gg.data = goal pub_goal.publish(gg) shoot = False if np.random.uniform(0, 1) > self.p_shoot: shoot = True shoot_length = np.random.randint(100, 800) action[0] = np.random.uniform(-1., 1.) action[1] = np.random.uniform(-1., 1.) for ep_step in range(self.episode_length): print( '[RL] Step %d in epoch %d, episode %d, state (%f,%f), goal (%f,%f), distance %f.' % (ep_step, epochs_count, n + 1, state[0], state[1], goal[0], goal[1], np.linalg.norm(state[:2] - goal))) total_step += 1 if not shoot: action = agent.choose_action([state], [goal], self.variance) # Act suc = move_srv(action[:2]).success rospy.sleep(0.05) rate.sleep() if suc: # Get observation next_state = np.array(obs_srv().state) fail = drop_srv( ).dropped # Check if dropped - end of episode else: # End episode if overload or angle limits reached rospy.logerr( '[RL] Failed to move gripper. Episode declared failed.' ) fail = True visited_states = np.append(visited_states, [next_state[:2]], axis=0) reward, done = self.transition_reward( next_state, goal, fail) ep_total_r += reward self.ep_experience.add(state, action, reward, next_state, done, goal) state = next_state if total_step % 100 == 0 or ep_step == self.episode_length - 1: if self.use_her: # The strategy can be changed here for t in range(len(self.ep_experience.memory)): for _ in range(self.K): future = np.random.randint( t, len(self.ep_experience.memory)) goal_ = self.ep_experience.memory[ future][ 3][:2] # next_state of future state_ = self.ep_experience.memory[t][ 0] action_ = self.ep_experience.memory[t][ 1] next_state_ = self.ep_experience.memory[ t][3] reward_, done_ = self.transition_reward( next_state_, goal_, False) self.ep_experience_her.add( state_, action_, reward_, next_state_, done_, goal_) agent.remember(self.ep_experience_her) self.ep_experience_her.clear() agent.remember(self.ep_experience) self.ep_experience.clear() self.variance *= 0.9998 a_loss, c_loss = agent.replay( self.optimization_steps) self.a_losses += [a_loss] self.c_losses += [c_loss] agent.update_target_net() if done: if reward == 0: print("[RL] Reached goal.") reached_goals = np.append(reached_goals, [goal], axis=0) else: failed_goals = np.append(failed_goals, [goal], axis=0) break if shoot and ep_step > shoot_length: shoot = False rate.sleep() successes += reward >= 0 and done self.success_rate.append(successes / float(self.num_episodes)) self.ep_mean_r.append(ep_total_r / float(self.num_episodes)) print("*** Epoch " + str(epochs_count + 1) + ", success rate " + str(self.success_rate[-1]) + ", ep_mean_r %.2f" % self.ep_mean_r[-1] + ", exploration %.2f" % self.variance) if len(self.success_rate) > 0 and len( self.ep_mean_r) > 0 and len(self.a_losses) > 0: agent.log2summary(total_episodes, self.success_rate[-1], self.a_losses[-1], self.ep_mean_r[-1]) else: plt.plot(visited_states[1:, 0], visited_states[1:, 1], '.b', label='visited states') plt.plot(failed_goals[1:, 0], failed_goals[1:, 1], '.r', label='failed goals') plt.plot(reached_goals[1:, 0], reached_goals[1:, 1], '.g', label='reached goals') plt.legend(loc='best') plt.xlabel('x') plt.ylabel('y') plt.show() if epochs_count > self.num_epochs: break rate.sleep()
import numpy as np import cv2 #from cv_bridge import CvBridge, CvBridgeError import sys #global num_omni.data num_omni = Int8() num_omni.data = 0 _opc_omni = Int8() _opc_omni.data = 0 _mov_omni = Char() _mov_omni.data = ord('K') _setpoint_omni = Float32MultiArray() _setpoint_omni.data = [0, 0, 0, 0] ###PUBLISH FUNCTIONS ##ENVIROMENT CAMERA BROKER-NODERED cam1 = String() cam1.data = " " #OMNI POSITION - BROKER-NODERED pos_net = Float32MultiArray() pos_net.data = [0, 0, 0, 0, 0, 0] #VELOCITY OF OMNIS LECTOR-NODERED rpm_net = Float32MultiArray() rpm_net.data = [0, 0, 0]
def main(portName, portBaud): print "HardwareHead.->INITIALIZING HEAD NODE..." ###Communication with dynamixels: global dynMan1 global goalPan global goalTilt print "HardwareHead.->Trying to open port on " + portName + " at " + str( portBaud) dynMan1 = Dynamixel.DynamixelMan(portName, portBaud) pan = 0 tilt = 0 i = 0 ### Set controller parameters dynMan1.SetDGain(1, 25) dynMan1.SetPGain(1, 16) dynMan1.SetIGain(1, 1) dynMan1.SetDGain(5, 25) dynMan1.SetPGain(5, 16) dynMan1.SetIGain(5, 1) ### Set servos features #dynMan1.SetMaxTorque(1, 1023) dynMan1.SetTorqueLimit(1, 512) #dynMan1.SetHighestLimitTemperature(1, 80) #dynMan1.SetMaxTorque(5, 1023) dynMan1.SetTorqueLimit(5, 512) #dynMan1.SetHighestLimitTemperature(5, 80) ###Connection with ROS rospy.init_node("head") br = tf.TransformBroadcaster() jointStates = JointState() jointStates.name = ["pan_connect", "tilt_connect"] jointStates.position = [0, 0] ## Subscribers subPosition = rospy.Subscriber("/hardware/head/goal_pose", Float32MultiArray, callbackPosHead) pubCurrentPose = rospy.Publisher("/hardware/head/current_pose", Float32MultiArray, queue_size=1) #subTorque = rospy.Subscriber("/torque", Float32MultiArray, callbackTorque) pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1) pubBatery = rospy.Publisher("/hardware/robot_state/head_battery", Float32, queue_size=1) msgCurrentPose = Float32MultiArray() msgCurrentPose.data = [0, 0] dynMan1.SetCWAngleLimit(5, 1023) dynMan1.SetCCWAngleLimit(5, 3069) dynMan1.SetCWAngleLimit(1, 0) dynMan1.SetCCWAngleLimit(1, 4095) dynMan1.SetGoalPosition(5, 2040) dynMan1.SetGoalPosition(1, 2520) dynMan1.SetTorqueEnable(5, 1) dynMan1.SetTorqueEnable(1, 1) dynMan1.SetMovingSpeed(5, 90) dynMan1.SetMovingSpeed(1, 90) loop = rospy.Rate(50) lastPan = 0.0 lastTilt = 0.0 goalPan = 0 goalTilt = 0 speedPan = 0.1 #These values should represent the Dynamixel's moving_speed speedTilt = 0.1 while not rospy.is_shutdown(): # Pose in bits #panPose = dynMan1.GetPresentPosition(5) #tiltPose = dynMan1.GetPresentPosition(1) # Pose in rad #if panPose != None: # pan = (panPose - 1750)*360/4095*3.14159265358979323846/180 # if abs(lastPan-pan) > 0.78539816339: # pan = lastPan #else: # pan = lastPan #if tiltPose != None: # tilt = (tiltPose - 970)*360/4095*3.14159265358979323846/180 # if abs(lastTilt-tilt) > 0.78539816339: # tilt = lastTilt #else: # tilt = lastTilt #SINCE READING IS NOT WORKING, WE ARE FAKING THE REAL SERVO POSE deltaPan = goalPan - pan deltaTilt = goalTilt - tilt if deltaPan > speedPan: deltaPan = speedPan if deltaPan < -speedPan: deltaPan = -speedPan if deltaTilt > speedTilt: deltaTilt = speedTilt if deltaTilt < -speedTilt: deltaTilt = -speedTilt pan += deltaPan tilt += deltaTilt jointStates.header.stamp = rospy.Time.now() jointStates.position[0] = pan #jointStates.position[1] = -tilt - 0.08 #goes upwards, but to keep a dextereous system, positive tilt should go downwards jointStates.position[ 1] = -tilt - 0.04 #goes upwards, but to keep a dextereous system, positive tilt should go downwards pubJointStates.publish( jointStates ) #We substract 0.1 to correct an offset error due to the real head position msgCurrentPose.data = [pan, tilt] pubCurrentPose.publish(msgCurrentPose) if i == 10: msgBatery = float(dynMan1.GetPresentVoltage(5) / 10.0) pubBatery.publish(msgBatery) i = 0 i += 1 lastPan = pan lastTilt = tilt loop.sleep()
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 goalSpine = 0.0 goalWaist = 0.0 goalShoulders = 0.0 spine = 0 waist = 0 shoulders = 0 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()
if saliency_map.value is None: image = bridge.value.imgmsg_to_cv2(image.value, "bgr8") image = np.zeros(image.shape) saliency_map.value = saliency.value.compute_saliency_map(image) saliency_map_current = saliency_map.value.copy() # apply curiosity camera_model.value.fromCameraInfo(camera_info_left.value) for point in points.value: # call service pixel = camera_model.value.project3dToPixel((point.point.x - pan.value, point.point.y - tilt.value, point.point.z)) x = int(pixel[0] * (len(saliency_map_current[0])/float(camera_info_left.value.width))) x = x + 6 # correction, bug in opencv? y = int(pixel[1] * (len(saliency_map_current)/float(camera_info_left.value.height))) if x >= 0 and x < len(saliency_map_current[0]) and y >=0 and y < len(saliency_map_current): from skimage.draw import circle rr, cc = circle(y, x, 25, (len(saliency_map_current), len(saliency_map_current[0]))) saliency_map[rr, cc] = saliency_map[rr, cc] * min(1, (t - point.header.stamp.to_sec())) saliency_map_image = bridge.value.cv2_to_imgmsg(np.uint8(saliency_map_current * 255.), "mono8") saliency_image_pub.value.publish(saliency_map_image) from std_msgs.msg import Float32MultiArray, MultiArrayDimension, MultiArrayLayout height = MultiArrayDimension(size=len(saliency_map_current)) width = MultiArrayDimension(size=len(saliency_map_current[0])) lo = MultiArrayLayout([height, width], 0) saliency_pub.value.publish(Float32MultiArray(layout=lo, data=saliency_map_current.flatten())) return
def stop_mission_callback(self, stop_mission): self.kill_switch = True path = Float32MultiArray() path.layout.data_offset = 3 path.data = [0, 0, 2] self.path_pub.publish(path)
def joyread(data): foo = Float32MultiArray() foo.data.append(4 * data.axes[1]) foo.data.append(4 * data.buttons[14]) pub.publish(foo)
def depth_callback(depth_message): global model global graph global prev_mp global ROBOT_Z global fx, cx, fy, cy with TimeIt('Crop'): depth = bridge.imgmsg_to_cv2(depth_message) # Crop a square out of the middle of the depth and resize it to 300*300 crop_size = 400 depth_crop = cv2.resize(depth[(480-crop_size)//2:(480-crop_size)//2+crop_size, (640-crop_size)//2:(640-crop_size)//2+crop_size], (300, 300)) # Replace nan with 0 for inpainting. depth_crop = depth_crop.copy() depth_nan = np.isnan(depth_crop).copy() depth_crop[depth_nan] = 0 with TimeIt('Inpaint'): # open cv inpainting does weird things at the border. depth_crop = cv2.copyMakeBorder(depth_crop, 1, 1, 1, 1, cv2.BORDER_DEFAULT) mask = (depth_crop == 0).astype(np.uint8) # Scale to keep as float, but has to be in bounds -1:1 to keep opencv happy. depth_scale = np.abs(depth_crop).max() depth_crop = depth_crop.astype(np.float32)/depth_scale # Has to be float32, 64 not supported. depth_crop = cv2.inpaint(depth_crop, mask, 1, cv2.INPAINT_NS) # Back to original size and value range. depth_crop = depth_crop[1:-1, 1:-1] depth_crop = depth_crop * depth_scale with TimeIt('Calculate Depth'): # Figure out roughly the depth in mm of the part between the grippers for collision avoidance. depth_center = depth_crop[100:141, 130:171].flatten() depth_center.sort() depth_center = depth_center[:10].mean() with TimeIt('Inference'): # Run it through the network. depth_crop = np.clip((depth_crop - depth_crop.mean()), -1, 1) with graph.as_default(): pred_out = model.predict(depth_crop.reshape((1, 300, 300, 1))) points_out = pred_out[0].squeeze() points_out[depth_nan] = 0 with TimeIt('Trig'): # Calculate the angle map. cos_out = pred_out[1].squeeze() sin_out = pred_out[2].squeeze() ang_out = np.arctan2(sin_out, cos_out)/2.0 width_out = pred_out[3].squeeze() * 150.0 # Scaled 0-150:0-1 with TimeIt('Filter'): # Filter the outputs. points_out = ndimage.filters.gaussian_filter(points_out, 5.0) # 3.0 ang_out = ndimage.filters.gaussian_filter(ang_out, 2.0) with TimeIt('Control'): # Calculate the best pose from the camera intrinsics. maxes = None ALWAYS_MAX = False # Use ALWAYS_MAX = True for the open-loop solution. if ROBOT_Z > 0.34 or ALWAYS_MAX: # > 0.34 initialises the max tracking when the robot is reset. # Track the global max. max_pixel = np.array(np.unravel_index(np.argmax(points_out), points_out.shape)) prev_mp = max_pixel.astype(np.int) else: # Calculate a set of local maxes. Choose the one that is closes to the previous one. maxes = peak_local_max(points_out, min_distance=10, threshold_abs=0.1, num_peaks=3) if maxes.shape[0] == 0: return max_pixel = maxes[np.argmin(np.linalg.norm(maxes - prev_mp, axis=1))] # Keep a global copy for next iteration. prev_mp = (max_pixel * 0.25 + prev_mp * 0.75).astype(np.int) ang = ang_out[max_pixel[0], max_pixel[1]] width = width_out[max_pixel[0], max_pixel[1]] # Convert max_pixel back to uncropped/resized image coordinates in order to do the camera transform. max_pixel = ((np.array(max_pixel) / 300.0 * crop_size) + np.array([(480 - crop_size)//2, (640 - crop_size) // 2])) max_pixel = np.round(max_pixel).astype(np.int) point_depth = depth[max_pixel[0], max_pixel[1]] # These magic numbers are my camera intrinsic parameters. x = (max_pixel[1] - cx)/(fx) * point_depth /1000 y = (max_pixel[0] - cy)/(fy) * point_depth /1000 z = point_depth.astype(np.float32) /1000 if np.isnan(z): return with TimeIt('Draw'): # Draw grasp markers on the points_out and publish it. (for visualisation) grasp_img = np.zeros((300, 300, 3), dtype=np.uint8) grasp_img[:,:,2] = (points_out * 255.0) grasp_img_plain = grasp_img.copy() rr, cc = circle(prev_mp[0], prev_mp[1], 5) grasp_img[rr, cc, 0] = 0 grasp_img[rr, cc, 1] = 255 grasp_img[rr, cc, 2] = 0 with TimeIt('Publish'): # Publish the output images (not used for control, only visualisation) grasp_img = bridge.cv2_to_imgmsg(grasp_img, 'bgr8') grasp_img.header = depth_message.header grasp_pub.publish(grasp_img) grasp_img_plain = bridge.cv2_to_imgmsg(grasp_img_plain, 'bgr8') grasp_img_plain.header = depth_message.header grasp_plain_pub.publish(grasp_img_plain) depth_pub.publish(bridge.cv2_to_imgmsg(depth_crop)) ang_pub.publish(bridge.cv2_to_imgmsg(ang_out)) # Output the best grasp pose relative to camera. cmd_msg = Float32MultiArray() cmd_msg.data = [x, y, z, ang, width, depth_center] print ("DATA: ", cmd_msg.data) cmd_pub.publish(cmd_msg)
def callback(data): global Rt2 global Rt1 global Ht1 global d1,d2 global theta1,theta2 global former_T global xv,yv lap_T = tw.lap() T = former_T + lap_T d1 = d2 = T * 0.86 #中心点の配信用 POINTS = [] #下にあるreceive_str_listをfloatにしたもの receive_float_list = [] #受信したデータを;で分けたもの receive_str_list = data.data.split(";") #float化 for rsl in receive_str_list: if rsl != "": receive_float_list.append([float(rs) for rs in rsl.split(",")]) #スイッチ、追跡対象確認できたらこれをTrueにしてfor文終了&座標送信 swb1 = False #number_list.data[0]が0の時、検知できたことを表し、1の時、検知失敗したことを表す。 number_list = Float32MultiArray() #受信データがある場合とない場合 #ある場合 if len(receive_float_list) > 0: receive_float_list = np.array(receive_float_list) #受信値の特徴データ receive_feature_list = receive_float_list[:,:feature_number] #受信値の中心座標データ receive_center_list = receive_float_list[:,feature_number:] #人間検知 result = clf_a.predict(receive_feature_list) #応急処置、中心点を始めに検知した方に向かう min_difference = 50 #足として検知したものの中心点をリスト化 #x,y,z,svmの結果 detected_legs = filter(lambda a: a[:][3] == 1, np.c_[receive_center_list,result]) detected_legs_pair = [] #足のペア検索 for k in range(len(detected_legs)-1): l = k+1 while l < len(detected_legs): point_difference = em.distanceBetweenTwoPoints(detected_legs[k],detected_legs[l]) if 0.15 <= point_difference and point_difference <= 0.35 : #x,y,z,足番号1,足番号2 detected_legs_pair.append([(detected_legs[k][0]+detected_legs[l][0])/2,(detected_legs[k][1]+detected_legs[l][1])/2,0,[detected_legs[k],detected_legs[l]]]) l+=1 #人間の可能性がある地点 if len(detected_legs_pair) > 0: for dlp in detected_legs_pair: POINTS.append([dlp[0],dlp[1],dlp[2],0xff0000])#赤点でペアの中央店を表示 d = em.distanceBetweenTwoPoints(dlp,Rt1) theta = em.twoVectorAngle(Rt1,dlp,Rt2,Rt1) #d1とtheta1の値(体に関する値)については今後埋める if d <= d1 and theta <= theta1: for l in range(2): #疑問:論文中にはbとあるが、それについての詳細が確認できていない for T in dlp[3]: d = em.distanceBetweenTwoPoints(Ht1[l],T) theta = em.twoVectorAngle(Ht1[l],T,Rt2,Rt1) #d2とtheta2の値(足に関する値)については今後埋める if d <= d2 and theta <= theta2: #xv = (dlp[0]-Rt1[0])/T #yv = (dlp[1]-Rt1[1])/T Ht1 = dlp[3] Rt2 = Rt1 Rt1 = dlp swb1 = True number_list.data = [0,dlp[0],dlp[1]] POINTS.append([dlp[0],dlp[1],dlp[2],0xff0000]) former_T = 0 #breakを入れる必要があるのか検討 if swb1: break if swb1: break if swb1: break if not swb1: number_list.data = [1,0,0] if former_T <= 1: former_T += lap_T #配信 point_cloud = pc2.create_cloud(HEADER, FIELDS, POINTS) pub.publish(point_cloud) pub_status.publish(number_list) #ない場合 else: print("No data")
def handle_rb01_CB(self, data): ### MEASUREMENTS ### self.rb01.pose.position.x = data.pose.position.x self.rb01.pose.position.y = data.pose.position.y self.rb01.pose.position.z = data.pose.position.z self.rb01.pose.orientation.x = data.pose.orientation.x self.rb01.pose.orientation.y = data.pose.orientation.y self.rb01.pose.orientation.z = data.pose.orientation.z self.rb01.pose.orientation.w = data.pose.orientation.w worldPosrb01 = np.array([ self.rb01.pose.position.x, self.rb01.pose.position.y, self.rb01.pose.position.z ]) worldRotrb01 = np.array([ self.rb01.pose.orientation.x, self.rb01.pose.orientation.y, self.rb01.pose.orientation.z, self.rb01.pose.orientation.w ]) worldPosrb02 = np.array([ self.rb02.pose.position.x, self.rb02.pose.position.y, self.rb02.pose.position.z ]) worldRotrb02 = np.array([ self.rb02.pose.orientation.x, self.rb02.pose.orientation.y, self.rb02.pose.orientation.z, self.rb02.pose.orientation.w ]) ### STEP 1 - calculate rotation from world frame to camera frame ### camRotWorld = tf.transformations.quaternion_multiply( tf.transformations.quaternion_inverse(self.rb02RotCam), tf.transformations.quaternion_inverse(worldRotrb02)) ### STEP 2 - calculate position of quad in world frame ### worldPosQuad = self.qv_multi(worldRotrb01, self.rb01PosQuad) + worldPosrb01 ### STEP 3 - calculate position of camera in world frame ### worldPosCam = self.qv_multi(worldRotrb02, self.rb02PosCam) + worldPosrb02 ### STEP 4 - calculate position of quad in camera frame ### camPosQuad = self.qv_multi(camRotWorld, (worldPosQuad - worldPosCam)) ### STEP 5 - calculate position of quad in pixel frame ### self.restrictPixel = camPosQuad[2] self.pixelsPosQuad.data = np.matmul( np.reshape(self.cameraMatrix, (3, 3)), camPosQuad) self.pixelsPosQuad.data = np.divide(self.pixelsPosQuad.data, camPosQuad[2]) ### PUBLISH ### toPublish = Float32MultiArray() toPublish.data = camPosQuad self.posePublisher.publish(toPublish)
def main(path): global publisher_motor, publisher_ping, publisher_servo, publisher_odom global IR_THRESHOLD, CYCLE_TIME global pose2d_sparki_odometry global starting_x, starting_y, cycle_count, cost_ij, x_from, y_from, x_to, y_to # TODO: Init your node to register it with the ROS core rospy.init_node('obstaclefinder', anonymous=True) init() #generate engine commands #SPARKI_SPEED = 0.0278 # 100% speed in m/s SPARKI_SPEED = 0.0056 #SPARKI_SPEED = 0.0099 SPARKI_AXLE_DIAMETER = 0.085 # Distance between wheels, meters engine_command = [] p_x, p_y = path[0] theta = 0 #init robot position to initial location msg = pose2d_sparki_odometry msg.x, msg.y, msg.theta = p_x, p_y, theta publisher_odom.publish(msg) msg = Float32MultiArray() #print("x,y, t:", p_x, p_y, theta) for x, y in path[1:]: print("x,y,px,py", x, y, p_x, p_y) dist = math.sqrt(pow(p_x - x, 2) + pow(p_y - y, 2)) if dist > 0: b_err = math.atan2((y - p_y), (x - p_x)) - theta b_err = trim_angle(b_err) else: b_err = 0 # engine_command measured in [turning time, forward time] #engine_command.append([b_err * SPARKI_AXLE_DIAMETER / 2 / SPARKI_SPEED, dist / SPARKI_SPEED]) rot_time, fow_time = b_err * SPARKI_AXLE_DIAMETER / 2 / SPARKI_SPEED, dist / SPARKI_SPEED #p_x, p_y, theta = x, y, theta + b_err print("b_err,dist:", b_err, dist) #first correct b_err if rot_time < 0: msg.data = [-1.0, 1.0] else: msg.data = [1.0, -1.0] publisher_motor.publish(msg) t0 = rospy.Time.now().to_sec() t1 = t0 while t1 - t0 < abs(rot_time): publisher_sim.publish(Empty()) t1 = rospy.Time.now().to_sec() #correct b_err twice because this simulator is ridiculous b_err = math.atan2((y - p_y), (x - p_x)) - pose2d_sparki_odometry.theta b_err = trim_angle(b_err) rot_time = b_err * SPARKI_AXLE_DIAMETER / 2 / SPARKI_SPEED if rot_time < 0: msg.data = [-1.0, 1.0] else: msg.data = [1.0, -1.0] publisher_motor.publish(msg) t0 = rospy.Time.now().to_sec() t1 = t0 while t1 - t0 < abs(rot_time): publisher_sim.publish(Empty()) t1 = rospy.Time.now().to_sec() #correct distance error msg.data = [2.0, 2.0] publisher_motor.publish(msg) t0 = rospy.Time.now().to_sec() t1 = t0 while t1 - t0 < fow_time / 2: publisher_sim.publish(Empty()) t1 = rospy.Time.now().to_sec() p_x = pose2d_sparki_odometry.x p_y = pose2d_sparki_odometry.y theta = pose2d_sparki_odometry.theta print("end of loop: x,y,t:", p_x, p_y, theta) msg.data = [0.0, 0.0] publisher_motor.publish(msg) publisher_sim.publish(Empty()) print("done")
import numpy as np import rospy import time from std_msgs.msg import Float32MultiArray, Float32 from geometry_msgs.msg import Twist from DirectKinmatics import sensor_location from InverseKinmatics import joints_angles DEG2RAD = math.pi / 180 POS2RAD = (2 * math.pi) / 4096 RAD2POS = 4096 / (2 * math.pi) ZERO_OFFSET = 4096/2 joints_angles_rad = [0, 0, 0, 0, 0] temp = [0, 0, 0, 0, 0] angles_msg = Float32MultiArray() location = Float32MultiArray() q4 = [0, 0, 0, 0] nw_angles_pub = rospy.Publisher("write/angles", Float32MultiArray, queue_size=10) def joints_angle_read(data): global POS2RAD global ZERO_OFFSET global joints_angles_rad global temp global q4 temp = data.data joints_angles_rad = list(data.data)
dy = 0 # Es la variable donde se almacena el valor de a (alpha). a = 0 # Es la variable donde se almacena el valor de b (beta). b = 0 # Es la variable en donde se guarda el resultado de aplicar la funcion atan2 al triangulo formado por dy y dx. # Equivale al angulo que se forma en el triangulo formado por el punto actual y final. t = 0 # Es la constante kp. Debe ser mayor que 0 para que el sistema sea localmente estable. kp = 0.3 #0.4 # mayor que 0, antes era 0.1 # Es la constante ka. ka-kp debe ser mayor que 0 para que el sistema sea localmente estable. ka = 0.5 # 1 # ka-kp mayor que 0, antes era 0.5 # Es la constante kb. Debe ser menor a 0 para que el sistema sea localmente estable. kb = 0.01 # menor que 0, era negativo -0.1 # Es la variable utilizada para publicar en el topico motorsVel la velocidad de cada motor. mot = Float32MultiArray() # En esta se almacenan las velocidades de cada motor. mot.data = [0, 0] # Senal para saber si ya se ejecuto empezar = False umbralFin = math.pi / 6 # En este metodo se inicializa el nodo, se suscribe a los topicos necesarios, se crea la variable para publicar al # topico de motorsVel y tambien se lanza el nodo encargado de graficar. Ademas es el metodo encargado de realizar # las acciones de control necesarias segun la ruta dada para llevar el robot a la posicion final. def control(): global posicionActual, g, ruta, pubMot, arrivedP, p, umbralP, kp, kb, ka, empezar, pedal # Se crean el nodo rospy.init_node('nodoControl', anonymous=True)
def gradient_ascent(stage, unit_vector, i): if stage == 1: pose = pose1 elif stage == 2: pose = pose2 else: pose = pose3 if i == 0: pose.rotated = 0 continue_loop = True rotate = 0 failed = False currentx = pose.x currenty = pose.y currentz = pose.z new_x = 1 new_y = 1 new_z = 1 alpha = 1 cross = 1 past_t = pose.T()[:] step = 0 djy = 0 djx = 0 achieved = False count_switch = 0 r = rospy.Rate(100) print "starting loop" if pose.rotated == 0: while continue_loop == True and cross > 0.0048: if (pose.T() == past_t) == False: print "Vector Changed" continue_loop = False break DJ = pose.calc_dj(currentx, currenty, Lt, unit_vector) # if np.sign(djx) != np.sign(DJ[0]) and step > 0 and abs(currentx) < 0.1: # new_x = currentx + djx*alpha # count_switch += 1 # else: if np.sign(djx) != np.sign(DJ[0]) and step > 0: count_switch += 1 new_x = currentx + DJ[0] * alpha djx = DJ[0] if np.sign(djy) != np.sign( DJ[1]) and step > 0 and abs(currenty) < 0.1: new_y = currenty + djy * alpha count_switch += 1 if np.sign(djy) != np.sign(DJ[1]) and step > 0: count_switch += 1 new_y = currenty + DJ[1] * alpha djy = DJ[1] new_z = currentz step += 1 currentx = new_x currenty = new_y currentz = new_z cross = crossb(currentx, currenty, unit_vector, Lt) print step, i, currentx, DJ[0], currenty, DJ[1], count_switch #Publishes every cycle of the gradient ascent to get the robot moving while #if x is small, then rotate and do gradient ascent again. perhaps in wp_setup if (step > 20000 or count_switch > 20) and i > 0: # if step > 20000 and i > 0: print "im broken" continue_loop = False rotate = 1 pose.rotated = 1 print step, count_switch, i break elif (step > 20000 or count_switch > 20) and i == 0: print "first point didn't converge" continue_loop = False break #r.sleep() if continue_loop == True: pose.updateXYZ(currentx, currenty, currentz, Lt, rotate) if pose.rotated > 0: count = 0 count_loop = 0 while count < 4 and count_loop < 2 and achieved == False: #run while loop again by multiplying T, xyz, and b by rotation matrix #send xyz with a 1 at the end of the list for rotation #if close to 0, rotation will still be in the small radius near 0 print step, i, pose.x, pose.y, pose.z, "before rotation" continue_loop = True failed = False if pose.x == 0: theta = 0 else: theta = np.arctan2(pose.y, pose.x) r = np.sqrt(pose.x**2 + pose.y**2) print theta, r, pose.rotated currentx = r * np.cos(theta + pose.rotated * 120 * np.pi / 180) currenty = r * np.sin(theta + pose.rotated * 120 * np.pi / 180) unit_vector_rot = T_rotated(unit_vector, pose.rotated) # currentx = pose.x*-0.5+pose.y*np.sin(120/180*np.pi) # currenty = -pose.x*np.sin(120/180*np.pi)+pose.y*0.5 # currentz = pose.z print step, i, currentx, currenty, currentz, "after rotation" #need to rotate T #need to check if x, y, and z rotation correct new_x = 1 new_y = 1 new_z = 1 alpha = 1 past_t = pose.T()[:] cross = 1 step = 0 djy = 0 djx = 0 count_switch = 0 print "rotated" r = rospy.Rate(100) while continue_loop == True and cross > 0.0048: if (pose.T() == past_t) == False: "I broke the loop" continue_loop = False break DJ = pose.calc_dj(currentx, currenty, Lt, unit_vector_rot) # if np.sign(djx) != np.sign (DJ[0]) and step > 0 and abs(currentx) < 0.1: # new_x = currentx + djx*alpha # count_switch += 1 # else: if np.sign(djx) != np.sign(DJ[0]) and step > 0: count_switch += 1 new_x = currentx + DJ[0] * alpha djx = DJ[0] if np.sign(djy) != np.sign( DJ[1]) and step > 0 and abs(currenty) < 0.1: new_y = currenty + djy * alpha else: new_y = currenty + DJ[1] * alpha # else: if np.sign(djy) != np.sign(DJ[1]) and step > 0: count_switch += 1 if count_switch < 5: djy = DJ[1] new_z = currentz step += 1 currentx = new_x currenty = new_y currentz = new_z cross = crossb(currentx, currenty, unit_vector_rot, Lt) print step, i, currentx, currenty, currentz, pose.rotated #if x is small, then rotate and do gradient ascent again. perhaps in wp_setup if step > 30000 or count_switch > 20: print "im broken" continue_loop = False failed = True achieved = False pose.rotated += 1 # if pose.rotated < 3: # if count == 0: # pose.rotated += -1**(pose.rotated)*-1 # elif count == 1: # pose.rotated = 0 # elif count == 2: # pose.rotated = 3 # else: # pose.rotated = 1 # count = -1 # count_loop += 1 break #r.sleep() if failed == False: achieved = True count += 1 # return "failed" #publishes message once convergence on final goal #if failed == False: theta = np.arctan2(currenty, currentx) if pose.rotated > 0: if pose.x == 0: theta = 0 r = np.sqrt(currentx**2 + currenty**2) currentx = r * np.cos(theta - pose.rotated * 120 * np.pi / 180) currenty = r * np.sin(theta - pose.rotated * 120 * np.pi / 180) if failed == False: pose.updateXYZ(currentx, currenty, currentz, Lt, pose.rotated) xyz_grad = [ pose1.x, pose1.y, pose1.z, pose1.rotated, pose2.x, pose2.y, pose2.z, pose2.rotated, pose3.x, pose3.y, pose3.z, pose3.rotated ] rotated = 0 if failed == False: if np.sign(pose.T()[0]) == -1: theta = np.arctan2(currenty, currentx) + np.pi #- 60/180*np.pi rotated = 0 r = np.sqrt(currentx**2 + currenty**2) currentx = r * np.cos(theta - pose.rotated * 120 * np.pi / 180) currenty = r * np.sin(theta - pose.rotated * 120 * np.pi / 180) xyz_send = [ pose1.x, pose1.y, pose1.z, pose1.rotated, pose2.x, pose2.y, pose2.z, pose2.rotated, currentx, currenty, pose3.z, rotated ] if i == 0 or pose.send == True: xyz_msg = Float32MultiArray(data=xyz_send) ort_pub.publish(xyz_msg) # else: # pose.quit_loop = True # xyz = "failed" # if i == 9: #if rotate == 1: #graph_check(stage, pose.rotated) return xyz_grad, xyz_send
def hand_status(): node_name = rospy.get_name() pub = rospy.Publisher('hand_command', Float32MultiArray, queue_size=1) rate = rospy.Rate(10) # 10hz x = 0 y = 0 z = 0 while not rospy.is_shutdown(): x_p = x y_p = y z_p = z # Get the accelerometer of each axis, 1G = 9.8m/s x, y, z = SH.get_accelerometer_raw().values() # Low-pass filter of measurements x = 0.5 * x + 0.5 * x_p y = 0.5 * y + 0.5 * y_p z = 0.5 * z + 0.5 * z_p x = round(x, 3) y = round(y, 3) z = round(z, 3) if x > 0.2: set_pixels(DOWN_PIXELS, WHITE) set_pixels(UP_PIXELS, BLACK) # set_pixels(CENTER_PIXELS, BLACK) # Dir_turn = "R" # SH.show_message("R") elif x < -0.2: set_pixels(UP_PIXELS, WHITE) set_pixels(DOWN_PIXELS, BLACK) # set_pixels(CENTER_PIXELS, BLACK) # Dir_turn = "L" # SH.show_message("L") else: set_pixels(UP_PIXELS, BLACK) set_pixels(DOWN_PIXELS, BLACK) # set_pixels(CENTER_PIXELS, WHITE) if y > 0.2: set_pixels(RIGHT_PIXELS, WHITE) set_pixels(LEFT_PIXELS, BLACK) # set_pixels(CENTER_PIXELS, BLACK) # Dir_go = "F" # SH.show_message("F") elif y < -0.2: set_pixels(LEFT_PIXELS, WHITE) set_pixels(RIGHT_PIXELS, BLACK) # set_pixels(CENTER_PIXELS, BLACK) else: set_pixels(LEFT_PIXELS, BLACK) set_pixels(RIGHT_PIXELS, BLACK) # set_pixels(CENTER_PIXELS, WHITE) # Dir_go = "B" # SH.show_message("B") # set_pixels(CENTER_PIXELS, WHITE) print("x=%.3f, y=%.3f, z=%.3f" % (x, y, z)) #hand_angle = Float32MultiArray (x, y, z) hand_angle = Float32MultiArray() hand_angle.data = [x, y, z] pub.publish(hand_angle) rate.sleep()
from std_msgs.msg import Float32MultiArray FIRE_JOINT_POSITIONS = (0, -53.5, 146.5, 0, 41.2, -8.73171) CATCH_JOINT_POSITIONS = (0, -95, 147, 0, 37.14258, -8.73171) CATCH_JOINT_POSITIONS_MESSAGE = Float32MultiArray(data=CATCH_JOINT_POSITIONS) FIRE_JOINT_POSITIONS_MESSAGE = Float32MultiArray(data=FIRE_JOINT_POSITIONS) ADVERSARY_RANDOM_J1_MIN = -27 ADVERSARY_RANDOM_J1_MAX = 27 ADVERSARY_RANDOM_J5_MIN = 25 ADVERSARY_RANDOM_J5_MAX = 65 Z1_S1_FRIENDLY = {"degrees_from_center": -10, "degrees_from_45": 0, "psi": 22} Z1_S2_FRIENDLY = {"degrees_from_center": 8, "degrees_from_45": 0, "psi": 22} Z2_S3_FRIENDLY = {"degrees_from_center": -14, "degrees_from_45": -5, "psi": 28} Z2_S4_FRIENDLY = {"degrees_from_center": 0, "degrees_from_45": -7, "psi": 28} Z2_S5_FRIENDLY = {"degrees_from_center": 16, "degrees_from_45": -5, "psi": 28} Z3_S6_FRIENDLY = { "degrees_from_center": -10, "degrees_from_45": -10, "psi": 35 } Z3_S7_FRIENDLY = {"degrees_from_center": -2, "degrees_from_45": -10, "psi": 35} Z3_S8_FRIENDLY = {"degrees_from_center": 4, "degrees_from_45": -10, "psi": 35} Z3_S9_FRIENDLY = {"degrees_from_center": 17, "degrees_from_45": -10, "psi": 35} Z1_S1_ADVERSARY = {
def main(): global publisher_motor, publisher_ping, publisher_servo, publisher_odom global IR_THRESHOLD, CYCLE_TIME global pose2d_sparki_odometry #DONE: Init your node to register it with the ROS core rospy.init_node('sparki', anonymous=True) init() rospy.Timer(rospy.Duration(10), display_map) # rospy.Timer(rospy.Duration(10), printArr) while not rospy.is_shutdown(): #DONE: Implement CYCLE TIME rate = rospy.Rate(1.0 / CYCLE_TIME) #Ping the ultrasonic publisher_ping.publish(Empty()) #Are updating map from within callback # rospy.loginfo("Ping:%s",PING_dist) # try: # convert_ultra_to_world(PING_dist) # except: # pass #DONE: Implement line following code here # To create a message for changing motor speed, use Float32MultiArray() # (e.g., msg = Float32MultiArray() msg.data = [1.0,1.0] publisher.pub(msg)) msg = Float32MultiArray() msg.data = [0.0, 0.0] if IR_right < IR_THRESHOLD and IR_right < IR_left: #publish to move right msg.data = [0.1, -1.0] #rospy.loginfo("Right Turn") elif IR_left < IR_THRESHOLD and IR_left < IR_right: #Publish to move left msg.data = [-1.0, 0.1] #rospy.loginfo("Left Turn") elif IR_center < IR_THRESHOLD and IR_left > IR_THRESHOLD and IR_right > IR_THRESHOLD: #Publish to move forward msg.data = [1.0, 1.0] #rospy.loginfo("Forward") # else: #Only added this because it wasnt working # msg.data = [0.0,0.0] # #rospy.loginfo("Default") # msg.data = [1.0,1.0] #For debugging purposes. publisher_motor.publish(msg) #DONE: Implement loop closure here if IR_right < IR_THRESHOLD and IR_left < IR_THRESHOLD and IR_center < IR_THRESHOLD: rospy.loginfo("Loop Closure Triggered") reset = Pose2D() reset.x = 0.0 reset.y = 0.0 reset.theta = 0.0 publisher_odom.publish(reset) #DONE: Implement CYCLE TIME rate.sleep()