def __init__(self): rospy.on_shutdown(self.cleanup) # publish command message to joints/servos of arm self.joint1 = rospy.Publisher('/waist_controller/command',Float64) self.joint2 = rospy.Publisher('/shoulder_controller/command',Float64) self.joint3 = rospy.Publisher('/elbow_controller/command',Float64) self.joint4 = rospy.Publisher('/wrist_controller/command',Float64) self.joint5 = rospy.Publisher('/hand_controller/command',Float64) self.pos1 = Float64() self.pos2 = Float64() self.pos3 = Float64() self.pos4 = Float64() self.pos5 = Float64() # Initial gesture of robot arm self.pos1 = 1.565 self.pos2 = 2.102 self.pos3 = -2.439 self.pos4 = -1.294 self.pos5 = 0.0 self.joint1.publish(self.pos1) self.joint2.publish(self.pos2) self.joint3.publish(self.pos3) self.joint4.publish(self.pos4) self.joint5.publish(self.pos5) while not rospy.is_shutdown(): # gesture 1 self.pos1 = 0.559 self.pos2 = 0.215 self.pos3 = -1.508 self.pos4 = -0.496 self.pos5 = 0.0 self.joint1.publish(self.pos1) self.joint2.publish(self.pos2) self.joint3.publish(self.pos3) self.joint4.publish(self.pos4) self.joint5.publish(self.pos5) rospy.sleep(2) # gesture 2 self.pos1 = 0.565 self.pos2 = -2.393 self.pos3 = -0.639 self.pos4 = 1.335 self.pos5 = -0.430 self.joint1.publish(self.pos1) self.joint2.publish(self.pos2) self.joint3.publish(self.pos3) self.joint4.publish(self.pos4) self.joint5.publish(self.pos5) rospy.sleep(3) # gesture 1 self.pos1 = 0.559 self.pos2 = 0.215 self.pos3 = -1.508 self.pos4 = -0.496 self.pos5 = 0.0 self.joint1.publish(self.pos1) self.joint2.publish(self.pos2) self.joint3.publish(self.pos3) self.joint4.publish(self.pos4) self.joint5.publish(self.pos5) rospy.sleep(2) # initial gesture self.pos1 = 0.565 self.pos2 = 2.102 self.pos3 = -2.439 self.pos4 = -1.294 self.pos5 = 0.0 self.joint1.publish(self.pos1) self.joint2.publish(self.pos2) self.joint3.publish(self.pos3) self.joint4.publish(self.pos4) self.joint5.publish(self.pos5) rospy.sleep(3)
[(50.0, 5.0, 0.0), (0.0, 0.0, 0.0, 1.0)], [(50.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)], [(40.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)], [(40.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)], [(30.0, 5.0, 0.0), (0.0, 0.0, 0.0, 1.0)], [(30.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)], [(20.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)], [(20.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)], [(10.0, 5.0, 0.0), (0.0, 0.0, 0.0, 1.0)], [(10.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)] ] result = Float64() result.data = 0 x_offset = 150 y_offset = 10 maxSimulations = 1 maxTime = 30*60; def goal_pose(pose): goal_pose = Odometry() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = 'world' goal_pose.pose.pose.position = Point(pose[0][0]+x_offset, pose[0][1]+y_offset, 0.) return goal_pose def get_result(result_aux): global result
def run(self): while not rospy.is_shutdown(): self.detectMarker() if self.data.appear: self.pub.publish(Float64(self.data.area)) print self.data
def main(): global args args = build_argparser().parse_args() model_xml = args.model model_bin = os.path.splitext(model_xml)[0] + ".bin" # ------------- 1. Plugin initialization for specified device and load extensions library if specified ------------- log.info("Creating Inference Engine...") ie = IECore() if args.cpu_extension and 'CPU' in args.device: ie.add_extension(args.cpu_extension, "CPU") # -------------------- 2. Reading the IR generated by the Model Optimizer (.xml and .bin files) -------------------- log.info("Loading network files:\n\t{}\n\t{}".format(model_xml, model_bin)) net = IENetwork(model=model_xml, weights=model_bin) print("model loaded") # ---------------------------------- 3. Load CPU extension for support specific layer ------------------------------ if "CPU" in args.device: supported_layers = ie.query_network(net, "CPU") not_supported_layers = [ l for l in net.layers.keys() if l not in supported_layers ] if len(not_supported_layers) != 0: log.error( "Following layers are not supported by the plugin for specified device {}:\n {}" .format(args.device, ', '.join(not_supported_layers))) log.error( "Please try to specify cpu extensions library path in sample's command line parameters using -l " "or --cpu_extension command line argument") sys.exit(1) assert len(net.inputs.keys( )) == 1, "Sample supports only YOLO V3 based single input topologies" # ---------------------------------------------- 4. Preparing inputs ----------------------------------------------- log.info("Preparing inputs") input_blob = next(iter(net.inputs)) # Defaulf batch_size is 1 net.batch_size = 1 # Read and pre-process input images n, c, h, w = net.inputs[input_blob].shape if args.labels: with open(args.labels, 'r') as f: labels_map = [x.strip() for x in f] else: labels_map = None is_async_mode = True #CHG!!! ''' CHG ''' wait_rate = rospy.Rate(1000) global rgb_img_update, depth_img_update, rgb_image, depth_img, bridge, motor_yaw_queue pub_total = rospy.Publisher("/yolo_ros_real_pose/detected_objects", ObjectsRealPose, queue_size=1) corresponding_img_pub = rospy.Publisher( "/yolo_ros_real_pose/img_for_detected_objects", Image, queue_size=1) pub_time = rospy.Publisher("/yolo_ros_real_pose/detection_time", Float64, queue_size=1) time_this = Float64() info_this = InfoGather() info_next = InfoGather() ''' END ''' if args.input == "ROS": while not rospy.is_shutdown(): if rgb_img_update == 1 and depth_img_update == 1 and len( rgb_image) != 0 and len(depth_img) != 0 and ( not motor_yaw_queue.empty() or not motor_yaw_syncronize): frame = rgb_image.copy() depth_this = depth_img.copy() info_this.uav_pose = uav_pose.pose info_this.img_to_pub = bridge.cv2_to_imgmsg( rgb_image, encoding="passthrough") info_this.img_to_pub.header = time_stamp_header if motor_yaw_syncronize: info_this.motor_yaw = motor_yaw_queue.get() else: info_this.motor_yaw = motor_yaw_corrected print("Time difference=" + str(uav_pose.header.stamp.to_sec() - time_stamp_header.stamp.to_sec())) rgb_img_update = 0 depth_img_update = 0 wait_key_code = 1 break else: input_stream = 0 if args.input == "cam" else args.input cap = cv2.VideoCapture(input_stream) number_input_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) number_input_frames = 1 if number_input_frames != -1 and number_input_frames < 0 else number_input_frames wait_key_code = 1 # Number of frames in picture is 1 and this will be read in cycle. Sync mode is default value for this case if number_input_frames != 1: ret, frame = cap.read() else: is_async_mode = False wait_key_code = 0 # ----------------------------------------- 5. Loading model to the plugin ----------------------------------------- log.info("Loading model to the plugin") exec_net = ie.load_network(network=net, num_requests=2, device_name=args.device) cur_request_id = 0 next_request_id = 1 render_time = 0 parsing_time = 0 # ----------------------------------------------- 6. Doing inference ----------------------------------------------- log.info("Starting inference...") print( "To close the application, press 'CTRL+C' here or switch to the output window and press ESC key" ) print( "To switch between sync/async modes, press TAB key in the output window" ) while not rospy.is_shutdown(): # Here is the first asynchronous point: in the Async mode, we capture frame to populate the NEXT infer request # in the regular mode, we capture frame to the CURRENT infer request try: detection_start_time = time() if args.input == "ROS": while not rospy.is_shutdown(): if rgb_img_update == 1 and depth_img_update == 1 and ( not motor_yaw_queue.empty() or not motor_yaw_syncronize): if len(rgb_image) != 0 and len(depth_img) != 0: if is_async_mode: next_frame = rgb_image.copy() depth_next = depth_img.copy() info_next.uav_pose = uav_pose.pose info_next.img_to_pub = bridge.cv2_to_imgmsg( rgb_image, encoding="passthrough") info_next.img_to_pub.header = time_stamp_header if motor_yaw_syncronize: info_next.motor_yaw = motor_yaw_queue.get() else: info_next.motor_yaw = motor_yaw_corrected else: frame = rgb_image.copy() depth_this = depth_img.copy() info_this.uav_pose = uav_pose.pose info_this.img_to_pub = bridge.cv2_to_imgmsg( rgb_image, encoding="passthrough") info_this.img_to_pub.header = time_stamp_header if motor_yaw_syncronize: info_this.motor_yaw = motor_yaw_queue.get() else: info_this.motor_yaw = motor_yaw_corrected print("Time difference=" + str(uav_pose.header.stamp.to_sec() - time_stamp_header.stamp.to_sec())) rgb_img_update = 0 depth_img_update = 0 break wait_rate.sleep() else: if is_async_mode: ret, next_frame = cap.read() else: ret, frame = cap.read() if not ret: break if is_async_mode: request_id = next_request_id in_frame = cv2.resize(next_frame, (w, h)) else: request_id = cur_request_id in_frame = cv2.resize(frame, (w, h)) # resize input_frame to network size in_frame = in_frame.transpose( (2, 0, 1)) # Change data layout from HWC to CHW in_frame = in_frame.reshape((n, c, h, w)) # Start inference start_time = time() exec_net.start_async(request_id=request_id, inputs={input_blob: in_frame}) det_time = time() - start_time # Collecting object detection results objects = list() if exec_net.requests[cur_request_id].wait(-1) == 0: output = exec_net.requests[cur_request_id].outputs start_time = time() for layer_name, out_blob in output.items(): out_blob = out_blob.reshape( net.layers[net.layers[layer_name].parents[0]].shape) layer_params = YoloParams(net.layers[layer_name].params, out_blob.shape[2]) log.info("Layer {} parameters: ".format(layer_name)) layer_params.log_params() objects += parse_yolo_region(out_blob, in_frame.shape[2:], frame.shape[:-1], layer_params, args.prob_threshold) parsing_time = time() - start_time # Filtering overlapping boxes with respect to the --iou_threshold CLI parameter objects = sorted(objects, key=lambda obj: obj['confidence'], reverse=True) for i in range(len(objects)): if objects[i]['confidence'] == 0: continue for j in range(i + 1, len(objects)): if intersection_over_union( objects[i], objects[j]) > args.iou_threshold: objects[j]['confidence'] = 0 # Drawing objects with respect to the --prob_threshold CLI parameter objects = [ obj for obj in objects if obj['confidence'] >= args.prob_threshold ] if len(objects) and args.raw_output_message: log.info("\nDetected boxes for batch {}:".format(1)) log.info( " Class ID | Confidence | XMIN | YMIN | XMAX | YMAX | COLOR " ) origin_im_size = frame.shape[:-1] current_total_dect = ObjectsRealPose() for obj in objects: # Validation bbox of detected object if obj['xmax'] > origin_im_size[1] or obj[ 'ymax'] > origin_im_size[0] or obj['xmin'] < 0 or obj[ 'ymin'] < 0: continue color = (int(min(obj['class_id'] * 12.5, 255)), min(obj['class_id'] * 7, 255), min(obj['class_id'] * 5, 255)) det_label = labels_map[obj['class_id']] if labels_map and len(labels_map) >= obj['class_id'] else \ str(obj['class_id']) if args.raw_output_message: log.info( "{:^9} | {:10f} | {:4} | {:4} | {:4} | {:4} | {} ". format(det_label, obj['confidence'], obj['xmin'], obj['ymin'], obj['xmax'], obj['ymax'], color)) cv2.rectangle(frame, (obj['xmin'], obj['ymin']), (obj['xmax'], obj['ymax']), color, 2) cv2.putText( frame, "#" + det_label + ' ' + str(round(obj['confidence'] * 100, 1)) + ' %', (obj['xmin'], obj['ymin'] - 7), cv2.FONT_HERSHEY_COMPLEX, 0.6, color, 1) '''CHG''' x_pos = 0 y_pos = 0 z_pos = 0 info = RealPose() info.label = det_label info.confidence = obj['confidence'] info.pix_lt_x = obj['xmin'] info.pix_lt_y = obj['ymin'] info.pix_rb_x = obj['xmax'] info.pix_rb_y = obj['ymax'] info.head_yaw = info_this.motor_yaw info.local_pose = info_this.uav_pose ### Now calculate the real position if len(depth_this) != 0 and (obj['xmax'] - obj['xmin']) * ( obj['ymax'] - obj['ymin']) >= 0: ### Calculate position here by depth image and camera parameters depth_box_width = obj['xmax'] - obj['xmin'] depth_box_height = obj['ymax'] - obj['ymin'] delta_rate = 0.1 x_box_min = int(obj['xmin'] + depth_box_width * delta_rate) y_box_min = int(obj['ymin'] + depth_box_height * delta_rate) x_box_max = int(obj['xmax'] - depth_box_width * delta_rate) y_box_max = int(obj['ymax'] - depth_box_height * delta_rate) after_width = (depth_box_width * (1 - 2 * delta_rate)) after_height = (depth_box_height * (1 - 2 * delta_rate)) ''' ''' # rect = depth_this[y_box_min:y_box_max, x_box_min:x_box_max] # Simulation rect = depth_this[y_box_min:y_box_max, x_box_min:x_box_max] * 0.001 rect[np.where(rect == 0)] = 99 rect[np.where(rect != rect)] = 99 z_pos = rect.min() x_pos = (0.5 * (obj['xmax'] + obj['xmin']) - cx) * z_pos / fx y_pos = (0.5 * (obj['ymax'] + obj['ymin']) - cy) * z_pos / fy info.x = x_pos info.y = y_pos info.z = z_pos current_total_dect.result.append(info) if len(current_total_dect.result) > 0: current_total_dect.header = info_this.img_to_pub.header pub_total.publish(current_total_dect) corresponding_img_pub.publish(info_this.img_to_pub) time_this.data = time() - detection_start_time pub_time.publish(time_this) '''END''' # Draw performance stats over frame inf_time_message = "Inference time: N\A for async mode" if is_async_mode else \ "Inference time: {:.3f} ms".format(det_time * 1e3) render_time_message = "OpenCV rendering time: {:.3f} ms".format( render_time * 1e3) async_mode_message = "Async mode is on. Processing request {}".format(cur_request_id) if is_async_mode else \ "Async mode is off. Processing request {}".format(cur_request_id) parsing_message = "YOLO parsing time is {:.3f}".format( parsing_time * 1e3) cv2.putText(frame, inf_time_message, (15, 15), cv2.FONT_HERSHEY_COMPLEX, 0.5, (200, 10, 10), 1) cv2.putText(frame, render_time_message, (15, 45), cv2.FONT_HERSHEY_COMPLEX, 0.5, (10, 10, 200), 1) cv2.putText(frame, async_mode_message, (10, int(origin_im_size[0] - 20)), cv2.FONT_HERSHEY_COMPLEX, 0.5, (10, 10, 200), 1) cv2.putText(frame, parsing_message, (15, 30), cv2.FONT_HERSHEY_COMPLEX, 0.5, (10, 10, 200), 1) start_time = time() cv2.imshow("DetectionResults", frame) # cv2.imshow("depth_this", depth_this) render_time = time() - start_time ''' For next ''' if is_async_mode: cur_request_id, next_request_id = next_request_id, cur_request_id frame = next_frame depth_this = depth_next info_this.uav_pose = info_next.uav_pose info_this.motor_yaw = info_next.motor_yaw info_this.img_to_pub = info_next.img_to_pub info_this.img_to_pub.header = info_next.img_to_pub.header key = cv2.waitKey(wait_key_code) # ESC key if key == 27: break # Tab key if key == 9: exec_net.requests[cur_request_id].wait() is_async_mode = not is_async_mode log.info("Switched to {} mode".format( "async" if is_async_mode else "sync")) except: print("An error occuered!!!!!!!") cv2.destroyAllWindows()
#!/usr/bin/env python from gazebo_msgs.msg import ODEPhysics from geometry_msgs.msg import Vector3 from gazebo_msgs.srv import SetPhysicsProperties from std_msgs.msg import Float64 import rospy set_gravity = rospy.ServiceProxy('/gazebo/set_physics_properties', SetPhysicsProperties) time_step = Float64(0.004) # default: 0.001 max_update_rate = Float64(250.0) # default: 1000.0 gravity = Vector3() gravity.x = 0.0 gravity.y = 0.0 gravity.z = -9.8 ode_config = ODEPhysics() ode_config.auto_disable_bodies = False ode_config.sor_pgs_precon_iters = 0 ode_config.sor_pgs_iters = 50 ode_config.sor_pgs_w = 1.3 ode_config.sor_pgs_rms_error_tol = 0.0 ode_config.contact_surface_layer = 0.001 ode_config.contact_max_correcting_vel = 50.0 # default: 100.0 ode_config.cfm = 0.0 ode_config.erp = 0.2 ode_config.max_contacts = 20 print "Changing gazebo properties." set_gravity(time_step.data, max_update_rate.data, gravity, ode_config)
if __name__ == '__main__': rospy.init_node('arm_start') arm1 = rospy.Publisher('arm_1_joint/command', Float64, queue_size=10) arm2 = rospy.Publisher('arm_2_joint/command', Float64, queue_size=10) arm3 = rospy.Publisher('arm_3_joint/command', Float64, queue_size=10) arm4 = rospy.Publisher('arm_4_joint/command', Float64, queue_size=10) arm5 = rospy.Publisher('arm_5_joint/command', Float64, queue_size=10) gripper = rospy.Publisher('gripper_1_joint/command', Float64, queue_size=10) rospy.wait_for_message('joint_states', JointState) for i in range(5): arm1.publish(Float64(0.0)) arm2.publish(Float64(0.0)) arm3.publish(Float64(-0.3)) arm4.publish(Float64(-0.65)) gripper.publish(Float64(0.15)) rospy.sleep(0.5) for i in range(5): gripper.publish(Float64(0.0)) rospy.sleep(0.5) for i in range(5): arm1.publish(Float64(0.0)) arm2.publish(Float64(0.0)) arm3.publish(Float64(0.0)) arm4.publish(Float64(0.0)) gripper.publish(Float64(0.0))
def __init__(self): rospy.on_shutdown(self.cleanup) # publish command message to joints/servos of arm self.joint1 = rospy.Publisher('/arm_shoulder_pan_joint/command', Float64) self.joint2 = rospy.Publisher('/arm_shoulder_lift_joint/command', Float64) self.joint3 = rospy.Publisher('/arm_elbow_flex_joint/command', Float64) self.joint4 = rospy.Publisher('/arm_wrist_flex_joint/command', Float64) self.joint5 = rospy.Publisher('/gripper_joint/command', Float64) self.ddtg = rospy.Publisher('toggle_msg', Float64) ##############33 self.pub = rospy.Publisher('/armOK', String) # Define a and b(a is the x-height and b is the y-height) self.a = Float64() self.b = Float64() self.x = Float64() self.y = Float64() self.rx = Float64() self.rz = Float64() # adding program self.nav = Float64( ) #the number which is used to get the order from node goforward # Subscribe three topics to get the x, y, z coordinate self.x = 10 self.y = 10 self.z = 10 # the variable for diandong tuigan self.grasp = 0 # adding program self.nav = 0 self.armsub = 0 self.a2 = 0.105 self.a3 = 0.105 self.a4 = 0.09 self.arm_length = 0.29 def callback_pos(data_pos): rospy.loginfo( rospy.get_caller_id() + "I heard that coordinate is %s", data_pos) self.grasp = 1 self.x = data_pos.x self.y = data_pos.y self.z = data_pos.z self.rx = self.x # adding program def callback_nav(data_nav): rospy.loginfo("I heard that the robot has reached the place") self.nav = data_nav print self.nav def callback_nav2(data_nav): if data_nav == "place": rospy.loginfo("I heard that the robot has reached point B") self.armsub = 1 else: self.armsub = 0 # adding program above rospy.Subscriber("image_pos", pos, callback_pos) rospy.loginfo("Subscribe to topic pos.....") rospy.sleep(2) #adding program rospy.Subscriber("nav_chatter", Float64, callback_nav) rospy.loginfo("Subscribe to topic nav_chatter.....") rospy.sleep(2) ###################### rospy.Subscriber("/nav", String, callback_nav2) rospy.loginfo("Subscribe to topic image2arm.....") rospy.sleep(2) # Define five poses self.pos1 = Float64() self.pos2 = Float64() self.pos3 = Float64() self.pos4 = Float64() self.pos5 = Float64() # Initial gesture of robot arm self.pos1 = 0 #1.57#4.14 self.pos2 = -1.1 self.pos3 = 2.1 self.pos4 = 1 self.pos5 = -0.4 self.joint1.publish(self.pos1) rospy.sleep(2) self.joint5.publish(self.pos5) rospy.sleep(2) self.joint4.publish(self.pos4) rospy.sleep(2) self.joint3.publish(2.1) rospy.sleep(2) self.joint2.publish(self.pos2) rospy.sleep(2) self.joint3.publish(self.pos3) rospy.sleep(2) rospy.loginfo("Connecting to turtlebot arm....") while not rospy.is_shutdown(): #rospy.loginfo("Test ......") if self.grasp == 1: self.a = self.x #used correct data before self.b = self.y self.obj_dis = math.sqrt(self.x**2 + self.y**2 + self.z**2) while self.arm_length < self.obj_dis: rospy.loginfo( "This coordinate is out of my location......") rospy.sleep(1) rospy.loginfo("The coordinate x we used to catch is ") print self.rx rospy.loginfo("The coordinate z we used to catch is ") print self.rz rospy.loginfo("X coordinate is ....") #print x coordinate print self.a rospy.loginfo("Y coordinate is ....") #print y coordinate print self.b if self.y < 0.27: self.y = self.y + 0.019 # calculate new joint angles self.theta1 = -math.atan2(self.x, self.y) self.l = math.hypot(self.x, self.y) - self.a4 self.l1 = math.hypot(self.l, self.z) self.cosfai = self.l1 / (2 * self.a2) rospy.loginfo("cosfai is ....") #print x coordinate print self.cosfai self.fai = math.acos(self.cosfai) self.theta = math.atan2(self.z, self.l) self.theta2 = math.pi / 2 - self.theta - self.fai self.theta4 = self.theta2 - math.acos(self.l1**2 / (2 * self.a2 * self.a3) - 1) self.theta3 = math.pi / 2 - self.theta2 - self.theta4 self.theta5 = 0.5 self.pos1 = self.theta1 self.pos2 = self.theta2 self.pos3 = self.theta3 self.pos4 = self.theta4 self.pos5 = self.theta5 rospy.loginfo("Calculate new joint angles ......") rospy.loginfo("The rotation angle of joint one.....") print self.theta1 rospy.loginfo("The rotation angle of joint two.....") print self.theta2 rospy.loginfo("The rotation angle of joint three.....") print self.theta3 rospy.loginfo("The rotation angle of joint four.....") print self.theta5 rospy.loginfo("The rotation angle of joint five.....") print self.theta5 # Rotate joint one to get the direction self.joint1.publish(self.pos1) rospy.sleep(2) # Stretched the arm self.joint5.publish(self.pos5) rospy.sleep(2) self.joint4.publish(-0.4) rospy.sleep(2) self.joint2.publish(0) rospy.sleep(2) self.joint3.publish(0.3) rospy.sleep(2) #self.joint3.publish(0.3) rospy.sleep(2) self.joint2.publish(self.pos2) rospy.sleep(2) self.joint3.publish(self.pos3) rospy.sleep(2) self.joint4.publish(self.pos4) rospy.sleep(2) # Catch the goods rospy.loginfo("Catch the goods ......") # Lift up the goods self.pos1 = 0 #1.57#4.14 self.pos2 = 1 self.pos3 = -1 self.pos4 = 0 self.pos5 = -0.3 self.joint5.publish(self.pos5) rospy.sleep(4) self.joint1.publish(self.pos1) self.joint2.publish(self.pos2) self.joint3.publish(self.pos3) self.joint4.publish(self.pos4) rospy.loginfo("Lift up the goods for 5 seconds ......") rospy.sleep(5) #15 before self.image = "go" self.pub.publish(self.image) rospy.loginfo( "I have grasped the item and please go to point B to place the item" ) if self.armsub == 1: rospy.loginfo("Ready for releasing the goods") # Release the goods self.pos1 = 0 self.pos1 = 0.7 self.pos3 = self.theta3 self.pos4 = self.theta4 self.pos5 = 0.6 self.joint1.publish(self.pos1) rospy.sleep(4) self.joint2.publish(self.pos2) self.joint3.publish(self.pos3) self.joint4.publish(self.pos4) rospy.sleep(4) self.joint5.publish(self.pos5) rospy.loginfo("Release the goods ......") rospy.sleep(4) self.armsub = 0 self.x = 10 self.z = 10
def control_knee(): global t_control global stimMsg global update_values global co_activation global new_current_quad global new_current_hams global control_sel global pw_min_h global pw_min_q global channels_sel global control_onoff global control_enable_quad global control_enable_hams global ilc_memory global ilc_i global Kp_now global Ki_now global Kd_now global ES_A_now global ES_omega_now global ES_phase_now global ES_K_now global ES_RC_now global ILC_alpha_now global ILC_beta_now global ILC_gama_now global initTime # init_variables t_control = [0, 0] # init control node controller = control.Control(rospy.init_node('control', anonymous=False)) # communicate with the dynamic server dyn_params = reconfig.Client('server', config_callback=server_callback) # subscribed topics sub = dict() sub.update({ 'pedal': rospy.Subscriber('imu/pedal', Imu, callback=imu_callback), 'ref': rospy.Subscriber('ref_channel', Float64, callback=reference_callback), 'steps': rospy.Subscriber('steps_channel', Float64, callback=steps_callback), 'curve': rospy.Subscriber('calibration_channel', Float64, callback=curve_callback) }) # sub = {} # sub['pedal'] = rospy.Subscriber('imu/pedal', Imu, callback=imu_callback) # sub['ref'] = rospy.Subscriber('ref_channel', Float64, callback=reference_callback) # sub['steps'] = rospy.Subscriber('steps_channel', Float64, callback=steps_callback) # published topics pub = dict() pub.update({ 'talker': rospy.Publisher('chatter', String, queue_size=10), 'control': rospy.Publisher('stimulator/ccl_update', Stimulator, queue_size=10), 'angle': rospy.Publisher('control/angle', Float64, queue_size=10), 'signal': rospy.Publisher('control/stimsignal', Int32MultiArray, queue_size=10) }) # define loop rate (in hz) rate = rospy.Rate(freq) # build basic angle message angleMsg = Float64() # errMsg = Float64() # build basic stimulator message stimMsg = Stimulator() stimMsg.mode = ['single', 'single', 'single', 'single'] stimMsg.pulse_width = [0, 0, 0, 0] stimMsg.pulse_current = [0, 0, 0, 0] stimMsg.channel = [1, 2, 3, 4] control_sel = 0 pw_min_h = 0 pw_min_q = 0 channels_sel = 0 control_onoff = False ESC_now = [0, 0, 0, 0, 0] ILC_now = [0, 0, 0] co_activation = False initTime = tempo.time() # load inverse dynamics try: ID_data = sio.loadmat(ID_path) id_pw = ID_data['id_pw'][0] id_angle = ID_data['id_angle'][0] # knee_ref = reference_data['knee_ref'][0] # loaded = 1 except: print("Ooops, the file you tried to load is not in the folder.") # knee_ref = [0, 0, 0, 0] # ---------------------------------------------------- start while not rospy.is_shutdown(): thisKnee = angle[-1] thisError = err_angle[-1] thisRef = refKnee[-1] thisTime = tempo.time() - initTime # ============================== >controllers start here new_kp = Kp_vector[-1] new_ki = Ki_vector[-1] new_kd = Kd_vector[-1] new_kp_hat = Kp_hat_vector[-1] new_ki_hat = Ki_hat_vector[-1] new_kd_hat = Kd_hat_vector[-1] newIntegralError = 0 new_u = 0 jcost = controller.jfunction(thisError, freq) # cost function if not co_activation: co_act_h = 0 co_act_q = 0 else: co_act_h = pw_min_h co_act_q = pw_min_q if control_onoff: # Open-loop inverse dynamics if control_sel == 0: try: res = next(x for x in id_angle if x > thisRef) # print list(id_angle).index(res) except StopIteration: res = 0 print "Not in recruitment curve" new_u = res / 500 # if thisError < 0: # new_u = -1 # else: # new_u = 1 # PID elif control_sel == 1: new_kp = Kp_now new_ki = Ki_now new_kd = Kd_now new_u, newIntegralError = controller.pid( thisError, u[-1], u[-2], integralError[-1], freq, [new_kp, new_ki, new_kd]) # PID-ILC elif control_sel == 2: new_kp = Kp_now new_ki = Ki_now new_kd = Kd_now # first do PID thisU_PID, newIntegralError = controller.pid( thisError, u[-1], u[-2], integralError[-1], freq, [new_kp, new_ki, new_kd]) # then ILC ilc_send[0] = ilc_memory[0][ilc_i] ilc_send[1] = ilc_memory[1][ilc_i] # only change new_u after the second cycle if ilc_memory[0][ilc_i] == 0: new_u = thisU_PID else: ILC_now[0] = ILC_alpha_now ILC_now[1] = ILC_beta_now ILC_now[2] = ILC_gama_now new_u = controller.pid_ilc(ilc_send, ILC_now, thisU_PID, ilc_i) if new_u > 1: new_u = 1 elif new_u < -1: new_u = -1 # update ilc_memory ilc_memory[0][ilc_i] = thisError ilc_memory[1][ilc_i] = new_u # update counter if ilc_i < len(ilc_memory[0]) - 1: ilc_i = ilc_i + 1 else: ilc_i = 0 # PID-ES elif control_sel == 3: # calculate new pid parameters using ES ESC_now[0] = ES_A_now ESC_now[1] = ES_omega_now ESC_now[2] = ES_phase_now ESC_now[3] = ES_RC_now ESC_now[4] = ES_K_now new_kp, new_kp_hat = controller.pid_es2( jcost, jcost_vector[-1], Kp_vector[-1], Kp_hat_vector[-1], 1 / freq, thisTime, ESC_now) # new kp ESC_now[0] = ES_A_now / 4 # ES_A_now # ESC_now[1] = ES_omega_now-2 ESC_now[2] = ES_phase_now + 0.1745 # ESC_now[3] = ES_RC_now/2 # ESC_now[4] = ES_K_now/2 new_ki, new_ki_hat = controller.pid_es2( jcost, jcost_vector[-1], Ki_vector[-1], Ki_hat_vector[-1], 1 / freq, thisTime, ESC_now) # new ki ESC_now[0] = ES_A_now / 16 # ES_A_now # ESC_now[1] = ES_omega_now-3 ESC_now[2] = ES_phase_now + 0.523 # ESC_now[3] = ES_RC_now/8 # ESC_now[4] = ES_K_now/8 new_kd, new_kd_hat = controller.pid_es2( jcost, jcost_vector[-1], Kd_vector[-1], Kd_hat_vector[-1], 1 / freq, thisTime, ESC_now) # new kd # do PID new_u, newIntegralError = controller.pid( thisError, u[-1], u[-2], integralError[-1], freq, [new_kp, new_ki, new_kd]) # CR elif control_sel == 4: # curve recruitment if control_enable_quad == 1: new_u = curveRecr[-1] elif control_enable_hams == 1: new_u = -curveRecr[-1] # no controller else: print("No controller was selected.") new_kp = 0 new_ki = 0 new_kd = 0 new_kp_hat = 0 new_ki_hat = 0 new_kd_hat = 0 jcost = 0 newIntegralError = 0 new_u = 0 if new_u > 1: new_u = 1 elif new_u < -1: new_u = -1 # update vectors Kp_vector.append(new_kp) Ki_vector.append(new_ki) Kd_vector.append(new_kd) Kp_hat_vector.append(new_kp_hat) Ki_hat_vector.append(new_ki_hat) Kd_hat_vector.append(new_kd_hat) jcost_vector.append(jcost) integralError.append(newIntegralError) u.append(new_u) # print refKnee[-1] # ============================== controllers end here # u to pw # new_pw = round(abs(u[-1] * 500) / 10) * 10 # define pw for muscles if u[-1] < 0: # new_pw = round(abs(u[-1]) * (500-pw_min_h) + pw_min_h) / 10 * 10 # u to pw new_pw = round(abs(u[-1]) * (500 - pw_min_h) + pw_min_h) # u to pw pw_h.append(new_pw) pw_q.append(co_act_q) controlMsg = "angle: %.3f, error: %.3f, u: %.3f (pw: %.1f) (Q), kp,ki,kd = { %.5f, %.5f, %.5f} : "\ % (thisKnee, thisError, u[-1], pw_q[-1], Kp_vector[-1], Ki_vector[-1], Kd_vector[-1]) else: new_pw = round(abs(u[-1]) * (500 - pw_min_q) + pw_min_q) # u to pw pw_q.append(new_pw) pw_h.append(co_act_h) controlMsg = "angle: %.3f, error: %.3f, u: %.3f (pw: %.1f) (H), kp,ki,kd = { %.5f, %.5f, %.5f} : "\ % (thisKnee, thisError, u[-1], pw_h[-1], Kp_vector[-1], Ki_vector[-1], Kd_vector[-1]) print(controlMsg) # define current for muscles current_q.append(new_current_quad) current_h.append(new_current_hams) # update topics if channels_sel == 1: # right leg stimMsg.pulse_width = [0, 0, pw_q[-1], pw_h[-1]] stimMsg.pulse_current = [0, 0, current_q[-1], current_h[-1]] else: # left leg stimMsg.pulse_width = [pw_q[-1], pw_h[-1], 0, 0] stimMsg.pulse_current = [current_q[-1], current_h[-1], 0, 0] pub['control'].publish(stimMsg) # send stim update angleMsg.data = thisKnee pub['angle'].publish(angleMsg) # send imu update # update timestamp # ts = tempo.time() t_control.append(thisTime) # next iteration rate.sleep() # ---------------------------------------------------- save everything # recreate vectors angle_err_lists = [angle, err_angle, t_angle] pid_lists = [Kp_vector, Ki_vector, Kd_vector, integralError, t_control] stim_lists = [current_q, current_h, pw_q, pw_h, t_control] ref_lists = [refKnee, t_ref, curveRecr, t_curve] ilc_lists = [ilc_memory, ILC_now, t_control] esc_lists = [ ESC_now, t_control, Kp_hat_vector, Ki_hat_vector, Kd_hat_vector, jcost_vector ] control_lists = [u, t_control] gui_lists = ([ t_gui, gui_enable_c, gui_control_sel, gui_step_time, gui_kp, gui_ki, gui_kd, gui_alpha, gui_beta, gui_gama, gui_A, gui_omega, gui_phase, gui_channels_sel, gui_pw_min_q, gui_pw_min_h, gui_K, gui_RC ]) path_lists = ([gui_ref_param]) steps_lists = ([t_steps, count_steps]) # create name of the file currentDT = datetime.datetime.now() # path_str = "/home/bb8/Desktop/gait_" path_str = gui_save_param[-1] if channels_sel == 1: # right leg leg_str = '_R_' else: # left leg leg_str = '_L_' if control_sel == 0: control_str = '0_' elif control_sel == 1: control_str = '1_' elif control_sel == 2: control_str = '2_' elif control_sel == 3: control_str = '3_' elif control_sel == 4: control_str = '4_' else: control_str = 'x_' if not co_activation: coact_str = "_noCA" else: coact_str = "_CA" currentDT_str = str(currentDT) name_file = path_str + coact_str + leg_str + control_str + currentDT_str + ".mat" # save .mat sio.savemat( name_file, { 'angle_err_lists': angle_err_lists, 'pid_lists': pid_lists, 'stim_lists': stim_lists, 'ref_lists': ref_lists, 'ilc_lists': ilc_lists, 'control_lists': control_lists, 'esc_lists': esc_lists, 'gui_lists': gui_lists, 'path_lists': path_lists, 'steps_lists': steps_lists })
rospy.init_node(name) host = rospy.get_param('~host') rate = rospy.get_param('~rate') ondotori_no = rospy.get_param('~ondotori_no') name_temp = node_name + ondotori_no + '_temp' name_hum = node_name + ondotori_no + '_hum' try: ondo = tr72w(host) except OSError as e: rospy.logerr("{e.strerror}. host={host}".format(**locals())) sys.exit() pub_temp = rospy.Publisher(name_temp, Float64, queue_size=1) pub_hum = rospy.Publisher(name_hum, Float64, queue_size=1) while not rospy.is_shutdown(): ret = ondo.measure() msg_temp = Float64() msg_hum = Float64() msg_temp.data = float(ret[0]) msg_hum.data = float(ret[1]) pub_temp.publish(msg_temp) pub_hum.publish(msg_hum) continue
self._limb.set_joint_positions(joint_angles) #self._limb.set_joint_velocities(cmd) diff = np.array(center) - np.array(desired_position) print('status=[{0},{1}]'.format(center[0],center[1])) print('desired=[{0},{1}]'.format(desired_position[0],desired_position[1])) print('desired=[{0},{1}]'.format(diff[0],diff[1])) if np.linalg.norm(diff) <= 7: self.step = self.step + 1 self.diff_old = diff pub_state = rospy.Publisher("/pixel_x/state",Float64,queue_size=1) msg_state = Float64(center[0]) pub_state.publish(msg_state) pub_state = rospy.Publisher("/pixel_y/state",Float64,queue_size=1) msg_state = Float64(center[1]) pub_state.publish(msg_state) pub_setpoint = rospy.Publisher("/pixel_x/setpoint",Float64,queue_size=1) msg_setpoint = Float64(desired_position[0]) pub_setpoint.publish(msg_setpoint) pub_setpoint = rospy.Publisher("/pixel_y/setpoint",Float64,queue_size=1) msg_setpoint = Float64(desired_position[1]) pub_setpoint.publish(msg_setpoint) else: print('search') current_pose = self._limb.endpoint_pose() if self.flag == 1: search_x = 0.5
def publisher(): rospy.init_node('imu') dataPub = rospy.Publisher('/imu/data', Imu, queue_size=3) infoPub = rospy.Publisher('/imu/info', bno055_info, queue_size=3) headingPub = rospy.Publisher('/imu/heading', Float64, queue_size=3) load_calibration = rospy.get_param("~load_calibration", False) rate = rospy.Rate(30) #30Hz data read # Setup BNO055 # Create and configure the BNO sensor connection. # Raspberry Pi configuration with I2C and RST connected to GPIO 27: global sensor sensor = BNO055.BNO055() try: sensor.begin() except Exception as e: rospy.logerr('Failed to initialize BNO055! %s', e) sys.exit(1) #sensor.set_axis_remap(BNO055.AXIS_REMAP_Y, BNO055.AXIS_REMAP_X, BNO055.AXIS_REMAP_Z) #swap x,y axis # Print system status and self test result. try: status, self_test, error = sensor.get_system_status() except Exception as e: rospy.logerr('Failed to read BNO055 system status! %s', e) sys.exit(2) rospy.logdebug('System status: %s', status) rospy.logdebug('Self test result (0x0F is normal): %s', hex(self_test)) # Print out an error if system status is in error mode. if status == 0x01: rospy.logerr('System error: %s', error) rospy.logerr('See datasheet section 4.3.59 for the meaning.') sys.exit(3) # Print BNO055 software revision and other diagnostic data. try: sw, bl, accel, mag, gyro = sensor.get_revision() except Exception as e: rospy.logerr('Failed to read BNO055 meta-inforamtion! %s', e) sys.exit(4) rospy.loginfo('Software version: %d', sw) rospy.loginfo('Bootloader version: %d', bl) rospy.loginfo('Accelerometer ID: %s', hex(accel)) rospy.loginfo('Magnetometer ID: %s', hex(mag)) rospy.loginfo('Gyroscope ID: %s', hex(gyro)) rospy.loginfo('Reading BNO055 data...') if load_calibration: try: sensor.set_calibration( np.load( os.path.join(os.path.dirname(os.path.realpath(__file__)), 'calibration.npy')).tolist()) except Exception as e: rospy.logerr("Error loading calibration data: " + str(e)) save_calibration_srv = rospy.Service('/imu/save_calibration', Trigger, save_calibration) while not rospy.is_shutdown(): # Define messages msg = Imu() info = bno055_info() heading = Float64() orientation = Quaternion() angular_vel = Vector3() linear_accel = Vector3() # Update meta data attempts = 0 sys_cal = 0 temp_c = 0 while attempts < 4: try: # Read the calibration status, 0=uncalibrated and 3=fully calibrated. sys_cal, gyro, accel, mag = sensor.get_calibration_status() temp_c = sensor.read_temp() break except Exception as e: rospy.logerr( 'Failed to read BNO055 calibration stat and temp! %s', e) attempts += 1 rospy.sleep(0.01) if attempts != 4: info.sysCalibration = sys_cal info.accelCalibration = accel info.gyroCalibration = gyro info.magnoCalibration = mag info.tempC = temp_c # Update real data attempts = 0 while attempts < 4: try: # Orientation as a quaternion: orientation.x, orientation.y, orientation.z, orientation.w = sensor.read_quaternion( ) # Gyroscope data (in degrees per second converted to radians per second): gry_x, gry_y, gry_z = sensor.read_gyroscope() angular_vel.x = math.radians(gry_x) angular_vel.y = math.radians(gry_y) angular_vel.z = math.radians(gry_z) # Linear acceleration data (i.e. acceleration from movement, not gravity-- # returned in meters per second squared): linear_accel.x, linear_accel.y, linear_accel.z = sensor.read_linear_acceleration( ) heading.data = sensor.read_euler()[0] break except Exception as e: rospy.logerr('Failed to read BNO055 data! %s', e) attempts += 1 rospy.sleep(0.01) if attempts != 4: msg.orientation = orientation msg.orientation_covariance[0] = 0.01 # covariance in x,y,z (3x3) msg.orientation_covariance[4] = 0.01 msg.orientation_covariance[8] = 0.01 msg.angular_velocity = angular_vel msg.angular_velocity_covariance[ 0] = 0.01 # covariance in x,y,z (3x3) msg.angular_velocity_covariance[4] = 0.01 msg.angular_velocity_covariance[8] = 0.01 msg.linear_acceleration = linear_accel msg.linear_acceleration_covariance[ 0] = 0.01 # covariance in x,y,z (3x3) msg.linear_acceleration_covariance[4] = 0.01 msg.linear_acceleration_covariance[8] = 0.01 #update message headers msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'imu_link' dataPub.publish(msg) info.header.stamp = rospy.Time.now() info.header.frame_id = 'imu_link' infoPub.publish(info) headingPub.publish(heading) rate.sleep()
def __init__(self, namespace, arm, slope): assert slope > 0, 'Slope cannot be negative' assert len(namespace) > 0, 'Namespace string is empty' self.sub = {} self.pub = {} self.ramp = {} self.slope = {} # Stores the last time stamp for the call for a ramp generation self.last_activation = {} # Joint limits self.joint_min = {} self.joint_max = {} # Starting time self.start_time = rospy.get_time() # Storing the robot's namespace self.namespace = namespace # Slope rate for the ramp self.slope_rate = slope if self.namespace[0] != '/': self.namespace = '/' + self.namespace if self.namespace[-1] != '/': self.namespace = self.namespace + '/' activeJointLimits = rospy.get_param(self.namespace + 'arms/' + arm + '/joint_limits') assert activeJointLimits is not None, 'Joint limits have not been loaded to the parameter server' self.jointNames = activeJointLimits.keys() self.jointPos = {} print 'RampGenerationNode::namespace=', self.namespace print 'RampGenerationNode::joints=', self.jointNames self.jointStateSub = rospy.Subscriber(self.namespace + 'joint_states', JointState, self.joint_state_callback) for joint in activeJointLimits: self.pub[joint] = rospy.Publisher(self.namespace + arm + '/' + joint + '/controller/command', Float64, queue_size=10) self.sub[joint] = rospy.Subscriber(self.namespace + arm + '/' + joint + '/ramp_generator/command', numpy_msg(Float64), self.ramp_callback, (joint)) # Set initial joint angle self.ramp[joint] = Float64() self.ramp[joint].data = 0.0 # Get joint limits self.joint_min[joint] = activeJointLimits[joint]['min'] self.joint_max[joint] = activeJointLimits[joint]['max'] assert self.joint_min[joint] < self.joint_max[joint], 'Invalid joint limit, joint=' + joint self.jointPos[joint] = None self.slope[joint] = 0.0 self.last_activation[joint] = self.start_time rate = rospy.Rate(500) jointPosInit = False print 'RampGenerationNode::start' while not rospy.is_shutdown(): # Read initial position of each joint for joint in self.jointNames: if self.jointPos[joint] is None and not jointPosInit: # print 'RampGenerationNode - No joint position available' continue elif self.jointPos[joint] is not None and not jointPosInit: self.ramp[joint].data = self.jointPos[joint] jointPosInit = True # If the control input has been deactivated for too long, set # slope to zero if rospy.get_time() - self.last_activation[joint] >= 0.5: self.slope[joint] = 0.0 self.ramp[joint].data = self.slope[joint] * rospy.get_time() + self.ramp[joint].data if self.ramp[joint].data < self.joint_min[joint]: self.ramp[joint].data = self.joint_min[joint] elif self.ramp[joint].data > self.joint_max[joint]: self.ramp[joint].data = self.joint_max[joint] self.pub[joint].publish(self.ramp[joint]) rate.sleep() print 'RampGenerationNode::end'
def pub_valve_angles(self, event): print 'Valves orientation: ', self.valve_0, ', ', self.valve_1, ', ', self.valve_2, ', ', self.valve_3 self.pub_valve_0.publish(Float64(self.valve_0)) self.pub_valve_1.publish(Float64(self.valve_1)) self.pub_valve_2.publish(Float64(self.valve_2)) self.pub_valve_3.publish(Float64(self.valve_3))
def listener_talker(): inicio = 0 #variable que indica si se leyeron los valores de negro y blanco antes de iniciar los motores SensorData = sensor_data() delay_palanca = 3.4 #--------Creacion de los nodos a utilizar---------- rospy.init_node( 'listener_talker_color', anonymous=True) # Creacion del nodo que se suscribe al sensor de color pub_motor_L = rospy.Publisher( '/ev3dev/OutPortA/command', Float64, queue_size=10 ) # the node is publishing to the /ev3dev/OutPortA/command topic, motorL velocity pub_motor_R = rospy.Publisher( '/ev3dev/OutPortB/command', Float64, queue_size=10 ) # the node is publishing to the /ev3dev/OutPortB/command topic, motorR velocity pub_motor_C = rospy.Publisher( '/ev3dev/OutPortC/command', Float64, queue_size=10 ) # the node is publishing to the /ev3dev/OutPortC/command topic, motorC velocity rospy.Subscriber( 'color', Illuminance, SensorData.iluminacion ) #To read the color from sensor, the node is subscribed to /color topic rospy.Subscriber('ultrasonic', Range, SensorData.distancia) rate = rospy.Rate(10) #Publicar----------------------------------------------- while not rospy.is_shutdown(): distancia_m = SensorData.dist print "distancia", distancia_m rospy.loginfo(distancia_m) if distancia_m <= 0.11: # and distancia_m <= 0.125: #the robot detect a box if SensorData.caja == 0: pub_motor_L.publish(Float64(0)) # The velocity of motor A pub_motor_R.publish(Float64(0)) # The velocity of motor B rospy.sleep(.5) # baja palanca pub_motor_C.publish(Float64(-1)) # The velocity of motor B rospy.sleep(delay_palanca) pub_motor_C.publish(Float64(0)) # The velocity of motor B rospy.sleep(1) # Avanza hacia adelante pub_motor_R.publish(Float64(1)) # The velocity of motor B pub_motor_L.publish(Float64(1)) # The velocity of motor A rospy.sleep(2) pub_motor_L.publish(Float64(0)) # The velocity of motor A pub_motor_R.publish(Float64(0)) # The velocity of motor B rospy.sleep(1) # sube la palanca pub_motor_C.publish(Float64(1)) # The velocity of motor C rospy.sleep(delay_palanca) pub_motor_C.publish(Float64(0)) # The velocity of motor C rospy.sleep(1) # gira hacia izquierda pub_motor_L.publish(Float64(-1.4)) # The velocity of motor A pub_motor_R.publish(Float64(1.4)) # The velocity of motor B rospy.sleep(2) SensorData.caja = 1 else: pub_motor_L.publish(Float64(0)) # The velocity of motor A pub_motor_R.publish(Float64(0)) # The velocity of motor B rospy.sleep(.5) # baja palanca pub_motor_C.publish(Float64(-1)) # The velocity of motor B rospy.sleep(delay_palanca) pub_motor_C.publish(Float64(0)) # The velocity of motor B rospy.sleep(1) # retrocede pub_motor_L.publish(Float64(-1)) # The velocity of motor A pub_motor_R.publish(Float64(-1)) # The velocity of motor B rospy.sleep(1.7) pub_motor_L.publish(Float64(0)) # The velocity of motor A pub_motor_R.publish(Float64(0)) # The velocity of motor B rospy.sleep(0.5) # sube la palanca pub_motor_C.publish(Float64(1)) # The velocity of motor C rospy.sleep(delay_palanca) pub_motor_C.publish(Float64(0)) # The velocity of motor C rospy.sleep(1) # gira hacia izquierda pub_motor_L.publish(Float64(-1.4)) # The velocity of motor A pub_motor_R.publish(Float64(1.4)) # The velocity of motor B rospy.sleep(2) SensorData.caja = 0 else: motor_v_L, motor_v_R = motors_velocity(SensorData.il, SensorData.dist) rospy.loginfo(motor_v_L) rospy.loginfo(motor_v_R) pub_motor_L.publish(Float64(motor_v_L)) # The velocity of motor A pub_motor_R.publish(Float64(motor_v_R)) # The velocity of motor B rate.sleep() #------------------------------------------------------- # spin() simply keeps python from exiting until this node is stopped rospy.spin()
servo_pub = rospy.Publisher(servo_state_topic, Float64, queue_size=1) vesc_state_pub = rospy.Publisher(motor_state_topic, VescStateStamped, queue_size=1) kmm = KinematicMotionModel(motor_state_topic, servo_state_topic, speed_to_erpm_offset, speed_to_erpm_gain, steering_angle_to_servo_offset, steering_angle_to_servo_gain, car_length, particles) # Give time to get setup rospy.sleep(1.0) # Send initial position and vesc state servo_msg = Float64() servo_msg.data = steering_angle_to_servo_gain * TEST_STEERING_ANGLE + steering_angle_to_servo_offset servo_pub.publish(servo_msg) rospy.sleep(1.0) vesc_msg = VescStateStamped() vesc_msg.header.stamp = rospy.Time.now() vesc_msg.state.speed = speed_to_erpm_gain * TEST_SPEED + speed_to_erpm_offset vesc_state_pub.publish(vesc_msg) rospy.sleep(TEST_DT) vesc_msg.header.stamp = rospy.Time.now() vesc_state_pub.publish(vesc_msg)
def people_callback(self, people): assert len(people.people) == 1 person = people.people[0] predictions = PeoplePrediction() markers = MarkerArray() markers.markers = [] predictions.predicted_people = [] # if the message stamp is empty, we assign the current time if people.header.stamp == rospy.Time(): people.header.stamp = rospy.get_rostime() #delta_t = [] tdist = 0.0 for goal in self.goals: goal.update_distance(person) dist = -(goal.heuristic_distance[-1] - goal.heuristic_distance[0]) dist = max(dist, 0.0) tdist += dist**2.0 #delta_t.append(dist) if tdist > 0.0: # to avoid singularities for goal in self.goals: dist = -(goal.heuristic_distance[-1] - goal.heuristic_distance[0]) dist = max(dist, 0.0) goal.probability = dist**2.0 / tdist #print(tdist, [goal.probability for goal in self.goals], sum([goal.probability for goal in self.goals])) #if sum(delta_t) > 0.0: # print(tdist, delta_t, sum(delta_t), delta_t[0]/sum(delta_t)) #else: # print(tdist, delta_t, sum(delta_t)) predictions.path_probabilities = probabilities = [] for goal in self.goals: goal.predict_path(person) probabilities.append(Float64()) probabilities[-1].data = goal.probability k = 0 for i in range(self.num_predictions): people_one_timestep = People() people_one_timestep.people = [] for goal in self.goals[::-1]: person_prediction = goal.path_prediction[i] people_one_timestep.people.append(person_prediction) prediction_marker = self.get_marker() prediction_marker.header.frame_id = people.header.frame_id prediction_marker.header.stamp = people.header.stamp prediction_marker.id = k k += 1 prediction_marker.pose.position = person_prediction.position # the opacity of the marker is adjusted according to the prediction step prediction_marker.color.a = ( 1 - i / float(self.num_predictions)) * goal.probability markers.markers.append(prediction_marker) # fill the header people_one_timestep.header.frame_id = people.header.frame_id people_one_timestep.header.stamp = people.header.stamp + \ rospy.Duration(i * self.time_resolution) # push back the predictions for this time step to the prediction container predictions.predicted_people.append(people_one_timestep) self.prediction_pub.publish(predictions) self.marker_pub.publish(markers)
def move_joints(self, roll_speed): joint_speed_value = Float64() joint_speed_value.data = roll_speed rospy.loginfo("Single Disk Roll Velocity>>"+str(joint_speed_value)) self._roll_vel_pub.publish(joint_speed_value)
def __init__(self): rospy.on_shutdown(self.cleanup) # publish command message to joints/servos of arm self.joint1 = rospy.Publisher('/arm_shoulder_pan_joint/command', Float64) self.joint2 = rospy.Publisher('/arm_shoulder_lift_joint/command', Float64) self.joint3 = rospy.Publisher('/arm_elbow_flex_joint/command', Float64) self.joint4 = rospy.Publisher('/arm_wrist_flex_joint/command', Float64) self.joint5 = rospy.Publisher('/gripper_joint/command', Float64) #self.ddtg = rospy.Publisher('toggle_msg',Float64) ##############33 self.grasped_pub = rospy.Publisher('/arm_grasped', String) self.released_pub = rospy.Publisher('/arm_released', String) # the variable for diandong tuigan self.grasp = -1 # adding program self.armsub = -1 def callback_nav(data_nav): if data_nav.data.find('release') > -1: rospy.loginfo("I heard that the robot has reached point B") self.armsub = 1 def followStopCallback(data_stop): if data_stop.data.find('follow_stop') > -1: rospy.loginfo("I heard that the robot has reached point A") self.grasp = 1 # adding program above rospy.Subscriber("ifFollowme", String, followStopCallback) rospy.Subscriber("nav2image", String, callback_nav) rospy.loginfo("Subscribe to topic nav_chatter.....") rospy.sleep(2) # Define five poses self.pos1 = Float64() self.pos2 = Float64() self.pos3 = Float64() self.pos4 = Float64() self.pos5 = Float64() # Initial gesture of robot arm self.pos1 = 1.4 #1.57#4.14 self.pos2 = 1.57 self.pos3 = -1.57 self.pos4 = 0.6 self.pos5 = -0.5 self.joint5.publish(self.pos5) rospy.sleep(1) self.joint4.publish(self.pos4) rospy.sleep(1) self.joint3.publish(self.pos3) rospy.sleep(1) #self.joint2.publish(0.55) #rospy.sleep(1) self.joint1.publish(self.pos1) rospy.sleep(1) self.joint2.publish(self.pos2) rospy.sleep(1) rospy.loginfo("Connecting to turtlebot arm....") while not rospy.is_shutdown(): #rospy.loginfo("Test ......") if self.grasp == 1 and self.armsub == -1: # Rotate joint one to get the direction self.joint2.publish(1) rospy.sleep(1) self.joint1.publish(0) rospy.sleep(1) # Stretched the arm self.joint5.publish(0.6) rospy.sleep(2) self.joint3.publish(-1.0) rospy.sleep(2) self.joint4.publish(-0.2) rospy.sleep(10) self.joint5.publish(-0.3) rospy.sleep(2) self.joint2.publish(2.0) self.joint3.publish(-2.0) rospy.sleep(2) #self.joint3.publish(0.3) rospy.loginfo("Lift up the goods for 5 seconds ......") self.ifgrasped = "grasped" self.grasped_pub.publish(self.ifgrasped) rospy.loginfo( "I have grasped the item and please go to point B to place the item" ) self.grasp = 0 if self.armsub == 1 and self.grasp == 0: rospy.loginfo("Ready for releasing the goods") # Release the goods self.pos1 = 0 self.pos2 = 1.4 self.pos3 = 1.2 self.pos4 = 1.2 self.pos5 = 0.6 self.joint1.publish(self.pos1) rospy.sleep(1) self.joint2.publish(self.pos2) self.joint3.publish(self.pos3) self.joint4.publish(self.pos4) rospy.sleep(1) self.joint5.publish(self.pos5) rospy.loginfo("Release the goods ......") rospy.sleep(3) self.armsub = 0 self.grasp = -1 self.ifreleased = "released" self.released_pub.publish(self.ifreleased) #reset the arm self.pos1 = 1.4 #1.57#4.14 self.pos2 = 1.57 self.pos3 = -1.57 self.pos4 = 0.6 self.pos5 = -0.5 self.joint5.publish(self.pos5) rospy.sleep(1) self.joint4.publish(self.pos4) rospy.sleep(1) self.joint3.publish(self.pos3) rospy.sleep(1) #self.joint2.publish(0.55) #rospy.sleep(1) self.joint1.publish(self.pos1) rospy.sleep(1) self.joint2.publish(self.pos2) rospy.sleep(1)
def calculateTwistCommand(self): lad = 0.0 #look ahead distance accumulator targetIndex = len(self.currentWaypoints.waypoints) - 1 for i in range(len(self.currentWaypoints.waypoints)): if ((i + 1) < len(self.currentWaypoints.waypoints)): this_x = self.currentWaypoints.waypoints[ i].pose.pose.position.x this_y = self.currentWaypoints.waypoints[ i].pose.pose.position.y next_x = self.currentWaypoints.waypoints[ i + 1].pose.pose.position.x next_y = self.currentWaypoints.waypoints[ i + 1].pose.pose.position.y lad = lad + math.hypot(next_x - this_x, next_y - this_y) if (lad > HORIZON): targetIndex = i + 1 break targetWaypoint = self.currentWaypoints.waypoints[targetIndex] targetSpeed = self.currentWaypoints.waypoints[0].twist.twist.linear.x targetX = targetWaypoint.pose.pose.position.x targetY = targetWaypoint.pose.pose.position.y currentX = self.currentPose.pose.position.x currentY = self.currentPose.pose.position.y print '[', targetX, targetY, ']' #get vehicle yaw angle quanternion = (self.currentPose.pose.orientation.x, self.currentPose.pose.orientation.y, self.currentPose.pose.orientation.z, self.currentPose.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quanternion) yaw = euler[2] #get angle difference alpha = math.atan2(targetY - currentY, targetX - currentX) - yaw print 'alpha = ', alpha l = math.sqrt( math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2)) if (l > 0.5): theta = math.atan(2 * L * math.sin(alpha) / l) print 'theta = ', theta self.ackermann_steering_control(targetSpeed, -theta) # #get twist command left_steer = Float64() right_steer = Float64() left_vel = Float64() right_vel = Float64() left_steer.data = left_angle right_steer.data = right_angle left_vel.data = left_speed right_vel.data = right_speed else: left_steer = Float64() right_steer = Float64() left_vel = Float64() right_vel = Float64() left_steer.data = 0 right_steer.data = 0 left_vel.data = 0 right_vel.data = 0 self.left_vel_pub.publish(left_vel) self.right_vel_pub.publish(right_vel) self.left_steer_pub.publish(left_steer) self.right_steer_pub.publish(right_steer)
def main(args): if len(sys.argv) < 2: print 'Please specify gps file' return 1 if len(sys.argv) < 3: print 'Please specify output rosbag file' return 1 gps = np.loadtxt(sys.argv[1], delimiter=",") utimes = gps[:, 0] modes = gps[:, 1] num_satss = gps[:, 2] lats = gps[:, 3] lngs = gps[:, 4] alts = gps[:, 5] tracks = gps[:, 6] speeds = gps[:, 7] bag = rosbag.Bag(sys.argv[2], 'w') try: for i, utime in enumerate(utimes): timestamp = rospy.Time.from_sec(utime / 1e6) status = NavSatStatus() if modes[i] == 0 or modes[i] == 1: status.status = NavSatStatus.STATUS_NO_FIX else: status.status = NavSatStatus.STATUS_FIX status.service = NavSatStatus.SERVICE_GPS num_sats = UInt16() num_sats.data = num_satss[i] fix = NavSatFix() fix.status = status fix.latitude = np.rad2deg(lats[i]) fix.longitude = np.rad2deg(lngs[i]) fix.altitude = alts[i] track = Float64() track.data = tracks[i] speed = Float64() speed.data = speeds[i] bag.write('fix', fix, t=timestamp) bag.write('track', track, t=timestamp) bag.write('speed', speed, t=timestamp) finally: bag.close() return 0
def callback(data): vel_bl_msg = Float64() vel_br_msg = Float64() vel_fl_msg = Float64() vel_fr_msg = Float64() position_x = data.pose.pose.position.x position_y = data.pose.pose.position.y print(position_x, ":", position_y) velocity_bl = 0 velocity_br = 0 if position_x < 0.65: velocity_bl = 0 velocity_br = 0 time.sleep(1) velocity_bl = 4 velocity_br = 4 time.sleep(0.1) velocity_bl = -8 velocity_br = -3 elif position_y < 0.35: velocity_bl = 0 velocity_br = 0 time.sleep(1) velocity_bl = 4 velocity_br = 4 time.sleep(0.1) velocity_bl = -8 velocity_br = -3 elif position_x > 2.4: velocity_bl = 0 velocity_br = 0 time.sleep(1) velocity_bl = 4 velocity_br = 4 time.sleep(0.1) velocity_bl = -3 velocity_br = -8 elif position_y > 2.2: velocity_bl = 0 velocity_br = 0 time.sleep(1) velocity_bl = 4 velocity_br = 4 time.sleep(0.1) velocity_bl = -3 velocity_br = -8 else: velocity_bl = random.uniform(-0.1, -10) velocity_br = random.uniform(-0.1, -10) time.sleep(0.5) velocity_fl = velocity_bl-1.0 velocity_fr = velocity_br-1.0 vel_bl_msg.data = velocity_bl vel_br_msg.data = velocity_br vel_fl_msg.data = velocity_fl vel_fr_msg.data = velocity_fr velocity_bl_publisher.publish(vel_fl_msg) velocity_br_publisher.publish(vel_fr_msg) velocity_fl_publisher.publish(vel_fl_msg) velocity_fr_publisher.publish(vel_fr_msg)
def node(): LIMIT_SWITCH_INIT_VEL = -15000 # counts per second print("Node starting...") global bot, delta_kin, lim_subs, limit_flags rospy.init_node('inverse_kinematics', anonymous=False) out_of_the_way_pub = rospy.Publisher('delta_out_of_the_way', Bool, queue_size=10) out_of_the_way_pub.publish(Bool(False)) bot = Bot() #bot.full_init(reset=False, k_p=400, k_d=60) #bot.set_gains(400, 60, perm=True) for axis in bot.axes: axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL axis.controller.vel_setpoint = 0 # bot.full_init() # for i in range(3): # bot.test_one(i, mytime=.1) delta_kin = kin.deltaSolver(realBot=realBot) # Limit switch init begins here limit_flags = [False for _ in range(3)] lim_subs = [ rospy.Subscriber('lim%d' % i, Bool, callback_lim_init(i)) for i in range(3) ] print("Beginning limit switch initialization...") for axis in bot.axes: axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL axis.controller.vel_setpoint = LIMIT_SWITCH_INIT_VEL while not all(limit_flags): rospy.sleep(.001) print("Limit switch initialization complete.") for axis in bot.axes: axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL axis.controller.vel_setpoint = 0 for axis in bot.axes: axis.controller.config.control_mode = CTRL_MODE_POSITION_CONTROL pub = rospy.Publisher('delta_position', PointStamped, queue_size=10) moving_pub = rospy.Publisher('delta_moving', Bool, queue_size=10) degrees_off_pub = rospy.Publisher('degrees_off', Point, queue_size=10) rospy.Subscriber('desired_position', PointStamped, move_to_point) current_pubs = [ rospy.Publisher('current%d' % i, Float64, queue_size=100) for i, _ in enumerate(bot.axes) ] out_of_the_way_pub = rospy.Publisher('delta_out_of_the_way', Bool, queue_size=10) out_of_the_way_point = PointStamped() out_of_the_way_point.point = Point(165, 75, -810) out_of_the_way_point.header.stamp = rospy.Time.now() out_of_the_way_point.header.frame_id = "robot_base_plate" move_to_point(out_of_the_way_point) send_robot_transform() rate = rospy.Rate(10) rospy.sleep(1) out_of_the_way_pub.publish(Bool(True)) while not rospy.is_shutdown(): bot.queueHandle() thetas = bot.get_rad_all() x, y, z = delta_kin.FK(thetas) pos = PointStamped() pos.point = Point(x=x, y=y, z=z) pos.header.stamp = rospy.Time.now() pos.header.frame_id = "robot_base_plate" pub.publish(pos) moving_pub.publish(Bool(bot.moving())) degrees_off_pub.publish(Point(*bot.get_enc_deg_dif_all())) for i, p in enumerate(current_pubs): p.publish(Float64(bot.axes[i].motor.current_control.Iq_measured)) rate.sleep()
def __init__(self): rospy.on_shutdown(self.cleanup) # publish command message to joints/servos of arm self.joint1 = rospy.Publisher('/arm_shoulder_pan_joint/command',Float64) self.joint2 = rospy.Publisher('/arm_shoulder_lift_joint/command',Float64) self.joint3 = rospy.Publisher('/arm_elbow_flex_joint/command',Float64) self.joint4 = rospy.Publisher('/arm_wrist_flex_joint/command',Float64) self.joint5 = rospy.Publisher('/gripper_joint/command',Float64) #self.ddtg = rospy.Publisher('toggle_msg',Float64) ##############33 self.arm_state_pub = rospy.Publisher('/arm_state', String) # Subscribe three topics to get the x, y, z coordinate self.x = 10 self.y = 10 self.z = 10 # the variable for diandong tuigan self.grasp = 0 # adding program self.armsub = 0 self.a2 = 0.105 self.a3 = 0.105 self.a4 = 0.09 self.arm_length = 0.3 self.label = -1 def callback_pos (data_pos): rospy.loginfo(rospy.get_caller_id() + "I heard that coordinate is %s", data_pos) self.grasp = 1 self.x = data_pos.x self.y = data_pos.y self.z = data_pos.z # adding program def callback_img (data_img):#if begin to grasp or release,from img progam if data_img.data.find('release') > -1: rospy.loginfo( "I heard that the robot has reached cupboard") self.armsub = 1 else: self.armsub = 0 def callback_label (data_label):#if begin to grasp or release,from img progam if data_label.data.find('zero') > -1: self.label = 0 if data_label.data.find('one') > -1: self.label = 1 if data_label.data.find('two') > -1: self.label = 2 if data_label.data.find('three') > -1: self.label = 3 else: self.label = -1 print data_label.data # adding program above rospy.Subscriber("image_pos", pos, callback_pos) rospy.loginfo("Subscribe to topic pos.....") rospy.sleep(1) #adding program rospy.Subscriber("obj_label", String, callback_label) rospy.loginfo("Subscribe to topic obj_label.....") rospy.sleep(1) ###################### rospy.Subscriber("img2arm_release", String, callback_img) rospy.loginfo("Subscribe to topic image2arm.....") rospy.sleep(1) # Define five poses self.pos1 = Float64() self.pos2 = Float64() self.pos3 = Float64() self.pos4 = Float64() self.pos5 = Float64() # Initial gesture of robot arm self.pos1 = 1.5#1.57#4.14 self.pos2 = 1.6 self.pos3 = -1.57 self.pos4 = 0.6 self.pos5 = -0.5 self.joint5.publish(self.pos5) rospy.sleep(1) self.joint4.publish(self.pos4) rospy.sleep(1) self.joint3.publish(self.pos3) rospy.sleep(1) #self.joint2.publish(0.55) #rospy.sleep(1) self.joint1.publish(self.pos1) rospy.sleep(1) self.joint2.publish(self.pos2) rospy.sleep(1) rospy.loginfo("Connecting to turtlebot arm....") while not rospy.is_shutdown(): if self.grasp == 1: self.obj_dis =math.sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2) while self.arm_length < self.obj_dis: rospy.loginfo("This coordinate is out of my location......") rospy.sleep(1) rospy.loginfo("The coordinate x we used to catch is ") print self.x rospy.loginfo("Y coordinate is ....") #print y coordinate print self.y rospy.loginfo("The coordinate z we used to catch is ") print self.z #if self.y < 0.25: # self.y = self.y + 0.019 # calculate new joint angles self.theta1 = - math.atan2(self.x,self.y) + 0.05 self.l = math.hypot(self.x,self.y) - self.a4 self.l1 = math.hypot(self.l,self.z) print self.l1 self.cosfai = self.l1 / (2 * self.a2) rospy.loginfo("cosfai is ....") #print x coordinate print self.cosfai if self.cosfai > 1: self.cosfai = 1 self.fai = math.acos(self.cosfai) self.theta = math.atan2(self.z,self.l) self.theta2 = math.pi / 2 - self.theta - self.fai self.theta4 = self.theta2 - math.acos(self.l1 ** 2 / (2 * self.a2 * self.a3) - 1 ) self.theta3 = math.pi / 2 - self.theta2 - self.theta4 self.theta5 = 0.8 self.pos1 = self.theta1 self.pos2 = self.theta2 self.pos3 = self.theta3 self.pos4 = self.theta4 self.pos5 = self.theta5 rospy.loginfo("Calculate new joint angles ......") rospy.loginfo("The rotation angle of joint one.....") print self.theta1 rospy.loginfo("The rotation angle of joint two.....") print self.theta2 rospy.loginfo("The rotation angle of joint three.....") print self.theta3 rospy.loginfo("The rotation angle of joint four.....") print self.theta5 rospy.loginfo("The rotation angle of joint five.....") print self.theta5 # Rotate joint one to get the direction self.joint2.publish(1.57) rospy.sleep(1) # Stretched the arm self.joint4.publish(-1.57) rospy.sleep(3) self.joint3.publish(-1.2) rospy.sleep(3) # Stretched the arm self.joint1.publish(self.pos1) rospy.sleep(3) self.joint5.publish(self.pos5) rospy.sleep(2) self.joint2.publish(self.pos2) rospy.sleep(2) self.joint3.publish(self.pos3) rospy.sleep(2) self.joint4.publish(self.pos4) rospy.sleep(3) # Catch the goods rospy.loginfo("Catch the goods ......") # Lift up the goods self.pos1 = 0#1.57#4.14 self.pos2 = 1.2 self.pos3 =-1.3 self.pos4 = -0.2 print self.label if self.label == 0: self.pos5 = -0.2 if self.label == 1: self.pos5 = -0.2 if self.label == 2: self.pos5 = 0 if self.label == 3: self.pos5 = -0.2 else : self.pos5 = -0.25 self.joint1.publish(self.pos1) self.joint2.publish(self.pos2) self.joint5.publish(self.pos5) rospy.sleep(5) self.joint3.publish(self.pos3) self.joint4.publish(self.pos4) rospy.loginfo("Lift up the goods for 5 seconds ......") rospy.sleep(5) #15 before self.ifgrasped = "grasped" self.arm_state_pub.publish(self.ifgrasped) rospy.loginfo("I have grasped the item and please go to point B to place the item") self.grasp = 0 if self.armsub == 1: rospy.loginfo("Ready for releasing the goods") # Release the goods self.pos1 = 0 self.pos2 = 0.8 self.pos3 = -0.7 self.pos4 = 1.2 self.pos5 = 0.6 self.joint1.publish(self.pos1) rospy.sleep(1) self.joint2.publish(self.pos2) rospy.sleep(1) self.joint3.publish(self.pos3) rospy.sleep(1) self.joint4.publish(self.pos4) rospy.sleep(1) self.joint5.publish(self.pos5) rospy.loginfo("Release the goods ......") rospy.sleep(5) self.armsub = 0 self.ifreleased = "released" self.arm_state_pub.publish(self.ifreleased) #reset the arm self.pos1 = Float64() self.pos2 = Float64() self.pos3 = Float64() self.pos4 = Float64() self.pos5 = Float64() # Initial gesture of robot arm self.pos1 = 1.5#1.57#4.14 self.pos2 = 1.57 self.pos3 = -1.57 self.pos4 = 0.6 self.pos5 = -0.5 self.joint5.publish(self.pos5) rospy.sleep(1) self.joint4.publish(self.pos4) rospy.sleep(1) self.joint3.publish(self.pos3) rospy.sleep(1) #self.joint2.publish(0.5) #rospy.sleep(1) self.joint1.publish(self.pos1) rospy.sleep(1) self.joint2.publish(self.pos2) rospy.sleep(1) rospy.loginfo("reset turtlebot arm....")
#!/usr/bin/env python import rospy from std_msgs.msg import Float64 rospy.init_node("min_pub") pub = rospy.Publisher("topic1", Float64, queue_size=1) rate = rospy.Rate(1) msg = Float64() msg.data = 0.0 while not rospy.is_shutdown(): pub.publish(msg) msg.data = msg.data + 0.1 rate.sleep()
def __init__(self): #### GoPiGo3 power management # export pin if not os.path.isdir("/sys/class/gpio/gpio"+self.POWER_PIN): gpio_export = os.open("/sys/class/gpio/export", os.O_WRONLY) os.write(gpio_export, self.POWER_PIN.encode()) os.close(gpio_export) time.sleep(0.1) # set pin direction gpio_direction = os.open("/sys/class/gpio/gpio"+self.POWER_PIN+"/direction", os.O_WRONLY) os.write(gpio_direction, "out".encode()) os.close(gpio_direction) # activate power management self.gpio_value = os.open("/sys/class/gpio/gpio"+self.POWER_PIN+"/value", os.O_WRONLY) os.write(self.gpio_value, "1".encode()) # GoPiGo3 and ROS setup self.g = gopigo3.GoPiGo3() print("GoPiGo3 info:") print("Manufacturer : ", self.g.get_manufacturer()) print("Board : ", self.g.get_board()) print("Serial Number : ", self.g.get_id()) print("Hardware version: ", self.g.get_version_hardware()) print("Firmware version: ", self.g.get_version_firmware()) self.reset_odometry() rospy.init_node("gopigo3") self.br = TransformBroadcaster() # subscriber rospy.Subscriber("motor/dps/left", Int16, lambda msg: self.g.set_motor_dps(self.ML, msg.data)) rospy.Subscriber("motor/dps/right", Int16, lambda msg: self.g.set_motor_dps(self.MR, msg.data)) rospy.Subscriber("motor/pwm/left", Int8, lambda msg: self.g.set_motor_power(self.ML, msg.data)) rospy.Subscriber("motor/pwm/right", Int8, lambda msg: self.g.set_motor_power(self.MR, msg.data)) rospy.Subscriber("motor/position/left", Int16, lambda msg: self.g.set_motor_position(self.ML, msg.data)) rospy.Subscriber("motor/position/right", Int16, lambda msg: self.g.set_motor_position(self.MR, msg.data)) rospy.Subscriber("servo/1", Float64, lambda msg: self.g.set_servo(self.S1, msg.data*16666)) rospy.Subscriber("servo/2", Float64, lambda msg: self.g.set_servo(self.S2, msg.data*16666)) rospy.Subscriber("cmd_vel", Twist, self.on_twist) rospy.Subscriber("led/blinker/left", UInt8, lambda msg: self.g.set_led(self.BL, msg.data)) rospy.Subscriber("led/blinker/right", UInt8, lambda msg: self.g.set_led(self.BR, msg.data)) rospy.Subscriber("led/eye/left", ColorRGBA, lambda c: self.g.set_led(self.EL, int(c.r*255), int(c.g*255), int(c.b*255))) rospy.Subscriber("led/eye/right", ColorRGBA, lambda c: self.g.set_led(self.ER, int(c.r*255), int(c.g*255), int(c.b*255))) rospy.Subscriber("led/wifi", ColorRGBA, lambda c: self.g.set_led(self.EW, int(c.r * 255), int(c.g * 255), int(c.b * 255))) # publisher self.pub_enc_l = rospy.Publisher('motor/encoder/left', Float64, queue_size=10) self.pub_enc_r = rospy.Publisher('motor/encoder/right', Float64, queue_size=10) self.pub_battery = rospy.Publisher('battery_voltage', Float64, queue_size=10) self.pub_motor_status = rospy.Publisher('motor/status', MotorStatusLR, queue_size=10) self.pub_odometry = rospy.Publisher("odometry", Odometry, queue_size=10) # services self.srv_reset = rospy.Service('reset', Trigger, self.reset) self.srv_spi = rospy.Service('spi', SPI, lambda req: SPIResponse(data_in=self.g.spi_transfer_array(req.data_out))) self.srv_pwr_on = rospy.Service('power/on', Trigger, self.power_on) self.srv_pwr_off = rospy.Service('power/off', Trigger, self.power_off) # main loop rate = rospy.Rate(10) # in Hz while not rospy.is_shutdown(): self.pub_enc_l.publish(Float64(data=self.g.get_motor_encoder(self.ML))) self.pub_enc_r.publish(Float64(data=self.g.get_motor_encoder(self.MR))) self.pub_battery.publish(Float64(data=self.g.get_voltage_battery())) # publish motor status, including encoder value (flags, power, encoder, speed) = self.g.get_motor_status(self.ML) status_left = MotorStatus(low_voltage=(flags & (1<<0)), overloaded=(flags & (1<<1)), power=power, encoder=encoder, speed=speed) (flags, power, encoder, speed) = self.g.get_motor_status(self.MR) status_right = MotorStatus(low_voltage=(flags & (1<<0)), overloaded=(flags & (1<<1)), power=power, encoder=encoder, speed=speed) self.pub_motor_status.publish(MotorStatusLR(header=Header(stamp=rospy.Time.now()), left=status_left, right=status_right)) # publish current pose (odom, transform)= self.odometry(status_left, status_right) self.pub_odometry.publish(odom) self.br.sendTransformMessage(transform) rate.sleep() self.g.reset_all() # deactivate power management os.write(self.gpio_value, "0".encode()) os.close(self.gpio_value) # unexport pin if os.path.isdir("/sys/class/gpio/gpio" + self.POWER_PIN): gpio_export = os.open("/sys/class/gpio/unexport", os.O_WRONLY) os.write(gpio_export, self.POWER_PIN.encode()) os.close(gpio_export)
def close_hand(): msg = Float64() msg.data = -1.0 for i in range(30): pub.publish(msg) rospy.sleep(0.1)
pub_cmd[10] = rospy.Publisher(pub_top[10], Float64) pub_cmd[11] = rospy.Publisher(pub_top[11], Float64) pub_cmd[12] = rospy.Publisher(pub_top[12], Float64) pub_cmd[13] = rospy.Publisher(pub_top[13], Float64) pub_cmd[14] = rospy.Publisher(pub_top[14], Float64) pub_cmd[15] = rospy.Publisher(pub_top[15], Float64) pub_cmd[16] = rospy.Publisher(pub_top[16], Float64) pub_cmd[17] = rospy.Publisher(pub_top[17], Pr2GripperCommand) pub_cmd[18] = rospy.Publisher(pub_top[18], Pr2GripperCommand) pub_cmd[19] = rospy.Publisher(pub_top[19], JointTrajectory) rospy.init_node(NAME, anonymous=False) rospy.Subscriber(sub_top[0], JointControllerState, positionState) cmd = Float64() cmd.data = -0.3 timeout = 10.0 while True: cmd.data = cmd.data * -1 start_time = time.time() end_time = start_time + timeout while time.time() < end_time: pub_cmd[0].publish(cmd) pub_cmd[1].publish(cmd) pub_cmd[2].publish(cmd) pub_cmd[3].publish(cmd) pub_cmd[4].publish(cmd) pub_cmd[5].publish(cmd)
def ReadMessages(): global carspeed global rise_time global settling_time global overshoot global ss_error global e_prior global es global first global bprev global bpprev global count global firstst global firstocl global firstss global cnt global now global setpoint1 global prev_time global first_time global set_po global car_sp global f global print_paras global control global cmn_first global stop_code global reset_values global count_log try: stsResult = PCAN_ERROR_OK while (not (stsResult & PCAN_ERROR_QRCVEMPTY)): result = m_objPCANBasic.Read(m_PcanHandle) stsResult = result[0] if result[0] == PCAN_ERROR_OK: newMsg = result[1] msgTimeStamp = result[2] #msgTimeStamp.value = (msgTimeStamp.micros + 1000 * msgTimeStamp.millis + 0x100000000 * 1000 * msgTimeStamp.millis_overflow) #print(format(newMsg.ID,'04x')) MsgID = format(newMsg.ID, '04x') DATA7 = format(newMsg.DATA[7], '02x') DATA6 = format(newMsg.DATA[6], '02x') if (MsgID == '076c'): #print "Vehicle speed status" print "\nSpeed of Car : ", int(DATA7, 16) carspeed1 = int(DATA7, 16) setpoint = Float64(setpoint1) carspeed = Float64(carspeed1) if (cmn_first == False): e_prior = float(setpoint1) - float(carspeed1) prev_time = rospy.get_time() first_time = rospy.get_time() car_sp.append([ float(carspeed1), float(setpoint1), prev_time - first_time ]) #set_po.append(float(setpoint1)) pub2.publish(setpoint) pub1.publish(carspeed) elif (control == True): now = rospy.get_time() - prev_time prev_time = rospy.get_time() e = float(setpoint1) - float(carspeed1) ed = (e - e_prior) * now es = (es + e * now) # if((float(carspeed1)/float(setpoint1)*100>90) and first==True): # rise_time=prev_time-first_time # first=False if (bprev - bpprev > 0 and carspeed1 - bprev < 0 and firstocl == True and bprev > setpoint): print "max", bprev #overshoot=(bprev-float(setpoint1))/float(setpoint1)*100 firstocl = False if (ed < 0.0001): count = count + 1 else: count = 0 if (count > 100): ss_error = e if (bprev - bpprev < 0.001 and carspeed1 - bprev < 0.001 and firstst == True): settling_time = prev_time - first_time if (cnt > 0.01): firstst = False cnt = cnt + 1 bpprev = bprev bprev = float(carspeed1) e_prior = e control = False car_sp.append([ float(carspeed1), float(setpoint1), prev_time - first_time ]) #set_po.append(float(setpoint1)) pub2.publish(setpoint) pub1.publish(carspeed) print "setpoint", setpoint elif (print_paras == True): print "rise_time", rise_time print "settling_time", settling_time print "overshoot", overshoot print "steady_state_error", ss_error print "current_carspeed", carspeed1 para_list = [ rise_time, settling_time, overshoot, ss_error, float(carspeed1) ] pickle.dump(car_sp, f) pickle.dump(para_list, f) l = open("full_pid_data1.txt", "a") l.write("\n" + count_log) l.write("\n" + str(car_sp)) count_log = count_log + 1 l.close() print_paras = False elif (reset_values == True): control = False rise_time = -100 settling_time = -100 overshoot = 0 ss_error = -100 e_prior = 0 es = 0 first = True bprev = 0 bpprev = 0 count = 0 firstst = True firstocl = True firstss = True cnt = 0 now = 0 prev_time = 0.0 first_time = 0.0 e2OCAN_RNDB.DATA[6] = RNDB_DRIVE e2OCAN_ACCEL.DATA[7] = ACCEL_ZERO e2OCAN_BRAKE.DATA[7] = BRAKE_ZERO reset_values = False #Logger= [carspeed , setpoint] #f=open("test.txt","a") #pickle.dump(Logger,f) #elif (MsgID == '076e'): #print "\n\nWiper & Horn Status" #if(DATA7 == 'a0'): #print "\nWiper : ON \nHorn : ON" #elif(DATA7 == '20'): #print "\nWiper : OFF \nHorn : ON" #elif(DATA7 == '80'): #print "\nWiper : ON \nHorn : OFF" #elif(DATA7 == '00'): #print "\nWiper : OFF \nHorn : OFF" #elif (MsgID == '0771'): #print "\n\nThrottle Status" #Throttle = DATA7 #print "\nThrottel Percentage of Car : ", int(Throttle,16)/2 #elif (MsgID == '0773'): #print "Braking Status" #Brake = DATA7 #print "\nBraking Percentage of Car : ", int(Brake,16)/2 elif (MsgID == '0775'): #print "\n\nSteering angle & Steering Direction Status" Steer = DATA7 Steer_bin = int(Steer, 16) #print Steer_bin #if((Steer_bin&3)==2): #print "\nSteering Direction : Clockwise" #elif((Steer_bin&3)==1): #print "\nSteering Direction : AntiClockwise" #print "angle:",Steer_bin>>2 #print "\n Steering Angle : ",Steer_angle #Steering Angle yet to be done #elif (MsgID == '0777'): #print "\n\nTurn Indicators & HL Beam" #if(DATA7[0] == '8'): #print "\nIndicator Status : Left Indicator" #elif(DATA7[0] == '1'): #print "\nIndicator Status : Right Indicator" #elif(DATA7[0] == '9'): #print "\nIndicator Status : Hazard Indicator" #elif(DATA7[0] == '0'): #print "\nIndicator Status : OFF" #if(DATA7[1] == '4'): #print "\nHead Lights : ON" #elif(DATA7[1] == '0'): #print "\nHead Lights : OFF" #elif (MsgID == '0779'): #print "\n\nDriving Mode & PRNDL Status" #if(DATA7 == '40'): #print "\nDriving Mode : Manual" #elif(DATA7 == '80'): #print "\nDriving Mode : Autonomous" #elif(DATA7 == 'c0'): #print "\nDriving Mode : Homing in Progress" #else: #print "\nDriving Mode : Invalid" #if(DATA6 == '08'): #print "\nGear Status : Reverse" #elif(DATA6 == '10'): #print "\nGear Status : Neutral" #elif(DATA6 == '20'): #print "\nGear Status : Drive" #elif(DATA6 == '80'): #print "\nGear Status : Boost" #else: #print "\nGear Status : Invalid" #print(msgTimeStamp.value) #for i in range(newMsg.LEN): # print(format(newMsg.DATA[i],'02x')) except KeyboardInterrupt: tmrRead.stop() m_objPCANBasic.Uninitialize(m_PcanHandle) raise SystemExit, 1
def move(self): self.pub.publish(Float64(self._goal_pos))
def __init__(self): # ROS parameters self.publish_frequency = rospy.get_param('~publish_frequency', 0.0) frame_id = rospy.get_param('~frame_id', '') child_frame_id = rospy.get_param('~child_frame_id', '') display_diff = rospy.get_param('~display_diff', True) display_RMSE = rospy.get_param('~display_RMSE', True) # Initialise variables self.odom1 = None self.odom2 = None sum_distance = 0.0 # sum of squared errors in position sum_orientation = 0.0 # sum of squared errors in orientation sum_linear_speed = 0.0 # sum of squared errors in linear speed sum_angular_speed = 0.0 # sum of squared errors in angular speed sum_nbr = 0.0 # nbr of errors in the sums # ROS publishers output_publisher = rospy.Publisher('diff_odom', Odometry, queue_size=1) RMSE_distance_publisher = rospy.Publisher('RMSE_distance', Float64, queue_size=1) RMSE_orientation_publisher = rospy.Publisher('RMSE_orientation', Float64, queue_size=1) RMSE_orientation_publisher = rospy.Publisher('RMSE_linear_speed', Float64, queue_size=1) RMSE_orientation_publisher = rospy.Publisher('RMSE_angular_speed', Float64, queue_size=1) # ROS subscribers rospy.Subscriber('odom1', Odometry, self.odom1_callback) rospy.Subscriber('odom2', Odometry, self.odom2_callback) # MAIN LOOP rate = rospy.Rate(self.publish_frequency) while not rospy.is_shutdown(): if (self.odom1 is not None) and (self.odom2 is not None): # Compute and publish the difference between the two odometries diff = Odometry() diff.header.stamp = rospy.get_rostime() diff.header.frame_id = frame_id diff.child_frame_id = child_frame_id diff.pose.pose.position.x = (self.odom1.pose.pose.position.x - self.odom2.pose.pose.position.x) diff.pose.pose.position.y = (self.odom1.pose.pose.position.y - self.odom2.pose.pose.position.y) diff_theta = self.compute_angle_difference( self.odom1.pose.pose.orientation, self.odom2.pose.pose.orientation) quaternion = tf.transformations.quaternion_from_euler( 0, 0, diff_theta) diff.pose.pose.orientation.x = quaternion[0] diff.pose.pose.orientation.y = quaternion[1] diff.pose.pose.orientation.z = quaternion[2] diff.pose.pose.orientation.w = quaternion[3] diff.twist.twist.linear.x = (self.odom1.twist.twist.linear.x - self.odom2.twist.twist.linear.x) diff.twist.twist.linear.y = (self.odom1.twist.twist.linear.y - self.odom2.twist.twist.linear.y) diff.twist.twist.angular.z = ( self.odom1.twist.twist.angular.z - self.odom2.twist.twist.angular.z) output_publisher.publish(diff) # Compute and publish the RMSE sum_distance += diff.pose.pose.position.x**2 + diff.pose.pose.position.y**2 sum_orientation += diff_theta**2 sum_linear_speed += diff.twist.twist.linear.x**2 + diff.twist.twist.linear.y**2 sum_angular_speed += diff.twist.twist.angular.z**2 sum_nbr += 1.0 RMSE_distance = sqrt(sum_distance / sum_nbr) RMSE_orientation = sqrt(sum_orientation / sum_nbr) RMSE_linear_speed = sqrt(sum_linear_speed / sum_nbr) RMSE_angular_speed = sqrt(sum_angular_speed / sum_nbr) RMSE_distance_publisher.publish(Float64(RMSE_distance)) RMSE_orientation_publisher.publish(Float64(RMSE_orientation)) RMSE_orientation_publisher.publish(Float64(RMSE_linear_speed)) RMSE_orientation_publisher.publish(Float64(RMSE_angular_speed)) if display_diff: rospy.loginfo('Diff odom:') rospy.loginfo( "d_x={:.3f} ; d_y={:.3f} ; d_theta={:.3f}".format( diff.pose.pose.position.x, diff.pose.pose.position.y, diff_theta)) rospy.loginfo( "d_v_x={:.3f} ; d_v_y={:.3f} ; d_w_z={:.3f}".format( diff.twist.twist.linear.x, diff.twist.twist.linear.y, diff.twist.twist.angular.z, )) if display_RMSE: rospy.loginfo('RMSE diff odom:') rospy.loginfo( "d: {:.3f} ; theta: {:.3f} ; v: {:.3f} ; omega: {:.3f}" .format(RMSE_distance, RMSE_orientation, RMSE_linear_speed, RMSE_angular_speed)) rate.sleep()