def __init__(self): self.speed = 0.0 self.turn = 0.0 self.lastUpdate = 0.0 rospy.init_node('robotControl', anonymous=True) rospy.Subscribe("cmd_vel",Twist,self.cmd_vel_callback) rospy.Subscribe("robotCommand",String,self.robCom_callback) self.telem_pub = rospy.Publish("telemetry",String,queue_size=10) self.robotCommand=rospy.Publish("robotCommand",String,queue_size=10)
def __init__(self): self.pub = rospy.Publisher('analglyph', Image) self.sub_depth = rospy.Subscribe('/kinect/depth/image_raw', Image, self.cb_depth) self.sub_rgb = rospy.Subscribe('/kinect/rgb/image_raw', Image, self.cb_rgb) self.bridge = CvBridge() self.img_out = cv.CreateMat(480, 640, cv.CV_8UC3) self.img_depth = cv.CreateMat(480, 640, cv.CV_8UC1) self.img_r = cv.CreateMat(480, 640, cv.CV_8UC1) self.img_g = cv.CreateMat(480, 640, cv.CV_8UC1) self.img_b = cv.CreateMat(480, 640, cv.CV_8UC1) self.img_rgb = cv.CreateMat(480, 640, cv.CV_8UC3)
def __init__(self): self.depth_subscriber = rospy.Subscribe("camera/depth/image_rect_raw", Image, self.callback_depth_image) self.bridge = CvBridge()
def __init__(self): feedback_start = False if rospy.has_param('start_tactile_feedback'): feedback_start = rospy.get_param('start_tactile_feedback') else: print("Waiting for forward teleoperation finished") if feedback_start: self.ff_pub = rospy.Publisher( "/sh_rh_ffj3_effort_controller/command", Float64, queue_size=10) self.mf_pub = rospy.Publisher( "/sh_rh_mfj3_effort_controller/command", Float64, queue_size=10) self.rf_pub = rospy.Publisher( "/sh_rh_rfj3_effort_controller/command", Float64, queue_size=10) self.lf_pub = rospy.Publisher( "/sh_rh_lfj3_effort_controller/command", Float64, queue_size=10) self.lf_pub = rospy.Publisher( "/sh_rh_lfj3_effort_controller/command", Float64, queue_size=10) rospy.Subscribe("rh/tactile_filtered", BiotacAll, self.tactile_callback) rospy.Subscribe("rh/tactile_zero", Float64, self.zero_callback) rospy.on_shutdown(self.release_tendon()) self.finger_name = ["ff", "mf", "rf", "lf", "th"] self.pacs = {"ff": [], "mf": [], "rf": [], "lf": [], "th": []} self.force_init = [0, 0, 0, 0, 0] if rospy.has_param('contact_info'): self.contact_info = rospy.get_param('contact_info') else: rospy.logerr("please set cotact info") self.slipping = False self.stable_grasp() rospy.spin()
def main(mtx, dist): cap = cv2.VideoCapture(0) # ID 1 had changed for test purposes. TODO:Must fix before URC aruco_dict = get_dictionary() parameters = aruco.DetectorParameters_create() parameters.markerBorderBits = 2 while not rospy.is_shutdown(): rospy.Subscribe('/stage_counter_topic', String, stage_callback) print("L: " + str(leftId) + " R: " + print(rightId)) ret, frame = cap.read() corners, ids, rejected_img_points = aruco.detectMarkers(gray, aruco_dict, parameters=parameters, cameraMatrix=mtx, distCoeff=dist) if np.all(ids is not None): # If there are markers found by detector for i in range(0, len(ids)): print(ids[i]) # Estimate pose of each marker and return the values rvec and tvec---different from camera coefficients rvec, tvec, markerPoints = aruco.estimatePoseSingleMarkers(corners[i], 0.02, mtx, dist) (rvec - tvec).any() # get rid of that nasty numpy value array error aruco.drawAxis(frame, mtx, dist, rvec, tvec, 0.01) aruco.drawDetectedMarkers(frame, corners) if len(ids) == 2: w = frame.shape[1] h = frame.shape[0] x,y,r = find_center(corners[0]) coordinatePublisher.publish(str(x) +","+ str(y) + "," + str(w) + "," + str(h)+ "," + str(r)) x,y,r = find_center(corners[1]) coordinatePublisher1.publish(str(x) +","+ str(y) + "," + str(w) + "," + str(h) +"," + str(r)) elif len(ids) == 1: w = frame.shape[1] h = frame.shape[0] x,y,r = find_center(corners[0]) coordinatePublisher.publish(str(x) +","+ str(y) + "," + str(w) + "," + str(h) + "," + str(r)) else: coordinatePublisher.Publish('-') coordinatePublisher1.Publish('-') cv2.imshow('frame', frame) dir_pub.Publish("1") key = cv2.waitKey(3) & 0xFF if key == ord('q'): # Quit break cap.release() cv2.destroyAllWindows()
def __init__(self): rospy.init_node('dbw_node') self.dbw_enabled = False self.linear_velocity = 0 self.angular_velocity = 0 self.current_velocity = 0 vehicle_mass = rospy.get_param('~vehicle_mass', 1736.35) fuel_capacity = rospy.get_param('~fuel_capacity', 13.5) brake_deadband = rospy.get_param('~brake_deadband', .1) decel_limit = rospy.get_param('~decel_limit', -5) accel_limit = rospy.get_param('~accel_limit', 1.) wheel_radius = rospy.get_param('~wheel_radius', 0.2413) wheel_base = rospy.get_param('~wheel_base', 2.8498) steer_ratio = rospy.get_param('~steer_ratio', 14.8) max_lat_accel = rospy.get_param('~max_lat_accel', 3.) max_steer_angle = rospy.get_param('~max_steer_angle', 8.) self.steer_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=1) self.throttle_pub = rospy.Publisher('/vehicle/throttle_cmd', ThrottleCmd, queue_size=1) self.brake_pub = rospy.Publisher('/vehicle/brake_cmd', BrakeCmd, queue_size=1) self.dbw_enable_sub = rospy.Subscribe('/vehicle/dbw_enabled', Boolean, dbw_enabled_callback) self.current_velocity_sub = rospy.Subscribe('/current_velocity', TwistStamped, current_velocity_callback) self.twist_cmd_sub = rospy.Subscribe('/twist_cmd', TwistStamped, twist_cmd_callback) # TODO: Create `TwistController` object self.controller = Controller(wheel_base, steer_ratio, 0, max_lat_accel, max_steer_angle) # TODO: Subscribe to all the topics you need to self.loop()
def main(): print("Usage: demo.py [model path] [image topic] [output topic]") model_path = sys.argv[1] image_topic_name = sys.argv[2] output_topic_name = sys.argv[3] if (len(sys.argv) < 4): print("arg error.") return pModel = load_model(model_path) rospy.init("GAAS_ImageDetection_TEGU_demo" + model_path) # which supports multi process. pPublisher = rospy.Publisher(output_topic_name) rospy.Subscribe(image_topic_name, 10, callback) rospy.spin()
def __init__(self): rospy.init_node('test_driver') # max velocity supported by motor controller/platform # velocity is in m/s self.maxSupportedVelocity = rospy.get_param("~maxSupportedVelocity", 2.2352) # m/s # max velocity the driver is permitted to drive # velocity is in m/s self.maxVelocity = rospy.get_param("~maxVelocity", 1) # acceleration is in % from 0 to 100% # for now, assume constant acceleration self.accel = rospy.get_param("~accel", 50) # min duration is the minimum duration to use # in a motor command...this will allow for us to creep/crawl forward if needed self.minDuration = rospy.get_param("~minDuration", 2) self.timeToTurn = rospy.get_param("~timeToTurn", 2) self.planSteps = [] self.mcPub = rospy.Publish("PhidgetMotor", MotorCommand) self.laPub = rospy.Publish("PhidgetLinear", LinearCommand) rospy.Subscribe('plan', Plan, handlePlan)
def listener(): rospy.init_node('listener') rospy.Subscribe("depth_topic",depth,callback) rospy.spin() ros
def listener(): rospy.init_node('grabber', anonymous=True) rospy.Subscribe("/cmd_vel", Twist, cmd_vel_callback) rospy.Subscribe("/camera/color/raw_image", Image, image_callback)
import rospy from std_msgs.msg import string def callback(dat): word = dat.data rospy.loginfo(word) if __name__ == '__main__': rospy.init_node("simple_publisher_node") try: rospy.Subscribe('/hello', String, callback) rospy.spin() except rospy.ROSInterruptException: pass
#!/usr/bin/env python import agv5Int import rospy posSubs = rospy.Subscribe('/robotPos',) if __name__ == '__main__':
def __init__(self, topic): # 라이다 클래스 실행시 할거 쓰셈 rospy.Subscribe(topic, LaserScan, self.callback_lidar) self.front = 0
forward_steer_topic = "FORWARD_STEER" global forward_steer global turn_offset forward_steer = 0 turn_offset = 0 def update_turn_offset(offset): global turn_offset turn_offset = offset rospy.loginfo("updated offset: %d", % (turn_offset)); def update_steer(steer): global forward_steer forward_steer = steer rospy.loginfo("updated velocity: %d", % (forward_steer)); if __name__ == '__main__': global forward_steer, turn_offset rospy.init_node(NODE_NAME) rospy.Subscribe(turn_offset_topic, Int32, update_turn_offset) rospy.Subscribe(forward_steer_topic, Int32, update_steer) i2c = smbus.SMBus(1) DEVICE_ADDRESS = 0x0a while True: i2c.write_byte(DEVICE_ADDRESS, 99) # vCom setting forward drive (0-255) i2c.write_byte(DEVICE_ADDRESS, 200 + forward_steer) # sCom setting steering (127 (standby), 126-0(max) left, 128-255(max) right) i2c.write_byte(DEVICE_ADDRESS, 127 + turn_offset) pass
left_side_step = Custom(json.setparse("21 Fst_L", offset=[darwin, hand])) right_side_step = Custom(json.setparse("20 Fst_R", offset=[darwin, hand])) l_step = Custom(json.setparse("24 F_E_L", offset=[darwin, hand])) r_step = Custom(json.setparse("25 F_E_R", offset=[darwin, hand])) kick = Custom(json.setparse("26 F_PShoot_R", offset=[darwin, hand])) #----------------------------------------------------------------------------------------------------------------------------------------------------- if __name__ == "__main__": ddxl = Dynamixel(lock=20) ddxl.angleWrite(19, 0) ddxl.angleWrite(20, 50) balance.run() raw_input("Proceed?") #rospy.init_node('Dash', anonymous=True) pub = rospy.Subscriber('shift', Float64, move, queue_size=1) pub_new = rospy.Subscribe('detect', String, det, queue_size=1) while True: if move > 0: left_side_step.run() if ret == "IR": break else: right_side_step.run() if ret == "IR": break rospy.spin()
def __init__(self): # set about print self.output_controller = False self.output_constant_PID = True # set constant value self.buoyancy = -0.0 self.first_time = True # set k pid for first time self.Kp_velocity = [0.7, 0.7, 0.7, 0.15, 0.15, 0.1] self.Ki_velocity = [0.05, 0.05, 0.0, 0.0, 0.0, 0.0] self.Kd_velocity = [0.0, 0.0, 0.0, 0.1, 0.1, 0.1] self.Kp_position = [0.7, 0.7, 3.0, 0.4, 0.3, 0.4] self.Ki_position = [0.1, 0.1, 0.1, 0.1, 0.5, 0.01] self.Kd_position = [0.2, 0.2, 0.2, 0.08, 0.2, 0.01] self.eq = expansion_quaternion() self.et = expansion_twist() self.error_position = numpy.array([0, 0, 0, 0, 0, 0]) # x, y, z, roll, pitch, yaw self.fix_state = [True, True, True, True, True, True] # for fix x, y, z, roll, pitch, yaw self.PID_position = [] self.PID_velocity = [] self.out_calculate = [] for i in range(0, 6): self.PID_position.append(PID()) self.PID_velocity.append(PID()) self.out_calculate.append(0) self.set_PID() self.fix_orientation = Quaternion(0, 0, 0, 1) rospy.init_node("controller") rate = rospy.Rate(10) # 10 hz #Listening rospy.Subscrive("topic name", variable type, function) rospy.Subscribe("/auv/state", Odometry, self.get_auv_state) #get current state rospy.Subscribe("/zeabus/cmd_vel", Twist, self.get_velocity) # get future state or goal rospy.Subscribe("/cmd_fix_position", Point, self.get_fix_position) # get x y z rospy.Subscribe("/cmd_fix_orientation", Quaternion, self.get_fix_orientation) # get rospy.Subscribe("/zeabus_controller/mode", Int16, self.get_mode) rospy.Subscribe("/fix/abs/depth", Float64, self.get_fix_depth) rospy.Subscribe("/fix/rel/yaw", Float64, self.get_real_yaw) rospy.Subscribe("/fix/abs/yaw", Float64, self.get_fix_yaw) rospy.Subscribe("/fix/rel/x", Float64, self.get_real_x) rospy.Subscribe("/switch/data", Switch, self.get_switch) #Use service rospy.Service("service name", service name, function) # self.result = rospy.Service("/fix_rel_x_srv", drive_x, self.request_xy) #return boolean get x y #declare variable for publisher rospy.Publisher('topic name', variable type, size) self.cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=10) # send to trust mapper self.fix_position_pubilsher = rospy.Publisher( "/zeabus_controller/is_at_fix_position", Bool, queue_size=10) # send for check True is success or fix position self.fix_orientation_pubilsher = rospy.Publisher( "/zeabus_controller/is_at_fix_orientation", Bool, queue_size=10) # same above but change position to orientation #declare about dynamic config and create # self.server = dynamic_reconfigure_server(PID_dynamic, self.configurePID) # ("ConfigType", function to recieve value) self.server = dynamic_reconfigure_server( PIDConstantConfig, self.configurePID) # ("ConfigType", function to recieve value)
def listener(): rospy.init_node("acceleration") rospy.Subscribe("/mavros/local_position/velocity", TwistStamped, velocity_update) rospy.spin()
def _uavGetHeight(self): rospy.Subscribe("/mavros/global_position/local", PoseWithCovarianceStamped, self.callbackHeight)
def LidarCheck(): global Lcheck rospy.Subscribe('/distance_sensor', Range, range_check)
import roslib roslib.load_manifest('learning_tf') import rospy import tf import turtlesim.msg def pose_callback: #do something -Broadcast transform br = tf.TransformBroadcaster(msg, turtlename) br.SendTransform((msg.x, msg.y, 0), tf.transformations.quaternion_from_euler(0, 0, msg.theta), rospy.Time.now(), turtlename, "world") if __name__ == "main": rospy.init_node("tf_broadcast") turtlename = rospy.get_param('~turtle') rospy.Subscribe('/%s/pose' % turtlename, turtlesim.msg.pose, pose_callback, turtlename) rospy.spin()