def __init__(self, sim=False): super(Core, self).__init__(sim) StateMachine.__init__(self) self.BC = Behavior() self.BK = Block() self.left_ang = 0 self.cp_value = 0 dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
def __init__(self, sim=False): super(Core, self).__init__(sim) StateMachine.__init__(self) self.CC = Chase() self.AC = Attack() self.BC = Behavior() self.left_ang = 0 self.dest_angle = 0 dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
def __init__(self): # nested dictionary of camera parameters adjustable through dynreconf gui self.cam_params = ['enable', 'refresh_rate', 'device_id', 'img_res'] params1 = { 'enable': False, 'refresh_rate': 30, 'device_id': 0, 'img_res': "1280x720" } params2 = { 'enable': False, 'refresh_rate': 30, 'device_id': 1, 'img_res': "1280x720" } self.cameras = {'camera_front_1': params1, 'camera_front_2': params2} # ROS messaging self.camera_front_1_enable_pub = rospy.Publisher( 'camera_front_1/enable', Bool, queue_size=1) self.camera_front_2_enable_pub = rospy.Publisher( 'camera_front_2/enable', Bool, queue_size=1) self.camera_front_1_refresh_rate_pub = rospy.Publisher( 'camera_front_1/refresh_rate', Int32, queue_size=1) self.camera_front_2_refresh_rate_pub = rospy.Publisher( 'camera_front_2/refresh_rate', Int32, queue_size=1) self.camera_front_1_device_id_pub = rospy.Publisher( 'camera_front_1/device_id', Int32, queue_size=1) self.camera_front_2_device_id_pub = rospy.Publisher( 'camera_front_2/device_id', Int32, queue_size=1) self.camera_front_1_img_res_pub = rospy.Publisher( 'camera_front_1/img_res', String, queue_size=1) self.camera_front_2_img_res_pub = rospy.Publisher( 'camera_front_2/img_res', String, queue_size=1) publishers1 = {'enable':self.camera_front_1_enable_pub, \ 'refresh_rate':self.camera_front_1_refresh_rate_pub, \ 'device_id':self.camera_front_1_device_id_pub, \ 'img_res':self.camera_front_1_img_res_pub, \ } publishers2 = {'enable':self.camera_front_2_enable_pub, \ 'refresh_rate':self.camera_front_2_refresh_rate_pub, \ 'device_id':self.camera_front_2_device_id_pub, \ 'img_res':self.camera_front_2_img_res_pub, \ } self.camera_publishers = { 'camera_front_1': publishers1, 'camera_front_2': publishers2 } # setup the callback for the reconfigure server self.server = DynamicReconfigureServer(mara_paramsConfig, self.reconfigure)
def __init__(self, robot_num, sim = False): super(Core, self).__init__(robot_num, sim) StateMachine.__init__(self) self.CC = Chase() self.AC = Attack() self.BC = Behavior() self.goal_dis = 0 self.tStart = time.time() self.block = 0 dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
def __init__(self): rospy.init_node("mission_planner", anonymous=False) self.modules = {} self.loadedStates = {} self.interact = Interaction() self.helper = [] self.missions = smach.StateMachine(outcomes=['pass', 'fail']) self.reconfigure_server = DynamicReconfigureServer( Config, self.cCallback) self.reconfigure_client = DynamicReconfigureClient("/DVL")
def __init__(self): # Pull constants from config file self.override = False self.lower = [0, 0, 0] self.upper = [0, 0, 0] self.min_trans = 0 self.max_velocity = 0 self.timeout = 0 self.min_observations = 0 self.camera = rospy.get_param('~camera_topic', '/camera/down/image_rect_color') self.goal = None self.last_config = None self.reconfigure_server = DynamicReconfigureServer( VampireIdentifierConfig, self.reconfigure) # Instantiate remaining variables and objects self._observations = deque() self._pose_pairs = deque() self._times = deque() self.last_image_time = None self.last_image = None self.tf_listener = tf.TransformListener() self.status = '' self.est = None self.visual_id = 0 self.enabled = False self.bridge = CvBridge() # Image Subscriber and Camera Information self.image_sub = Image_Subscriber(self.camera, self.image_cb) self.camera_info = self.image_sub.wait_for_camera_info() self.camera_info = self.image_sub.wait_for_camera_info() self.camera_model = PinholeCameraModel() self.camera_model.fromCameraInfo(self.camera_info) self.frame_id = self.camera_model.tfFrame() # Ros Services so mission can be toggled and info requested rospy.Service('~enable', SetBool, self.toggle_search) self.multi_obs = MultiObservation(self.camera_model) rospy.Service('~pose', VisionRequest, self.request_buoy) self.image_pub = Image_Publisher("drac_vision/debug") self.point_pub = rospy.Publisher("drac_vision/points", Point, queue_size=1) self.mask_image_pub = rospy.Publisher('drac_vision/mask', Image, queue_size=1) # Debug self.debug = rospy.get_param('~debug', True)
def __init__(self): rate = rospy.Rate(10) # 10hz # Create a dynamic reconfigure server. self.server = DynamicReconfigureServer(ConfigType, self.reconfigure) # Create a publisher for our custom message. pub = rospy.Publisher('/object_tracking_2d_ros/init_poses', ObjectDetections, queue_size=10) # Set the message to publish as our custom message. msg = ObjectDetections() # Initialize message variables. h = std_msgs.msg.Header() h.stamp = rospy.Time.now() p = Pose() p.position.x = 0 p.position.y = 0 p.position.z = 1 p.orientation.x = 0 p.orientation.y = 0 p.orientation.z = 0 p.orientation.w = 1 msg.header = h msg.detections.append(ObjectDetection()) msg.detections[0].id = "1" msg.detections[0].pose = p # Main while loop. while not rospy.is_shutdown(): # Fill in custom message variables with values from dynamic reconfigure server. p.position.x = self.tf_x p.position.y = self.tf_y p.position.z = self.tf_z p.orientation.x = self.rot_x p.orientation.y = self.rot_y p.orientation.z = self.rot_z p.orientation.w = self.rot_w h = std_msgs.msg.Header() h.stamp = rospy.Time.now() msg.header = h msg.detections[0].pose = p rospy.loginfo(msg) pub.publish(msg) rate.sleep()
def __init__(self,options=None): self.options = options self.warntime = time.time() self.USB = False self.joy_scale = [-1,1,-1,1,1] #RPYTY self.trim_roll = 0 self.trim_pitch = 0 self.max_angle = 30 self.max_yawangle = 200 self.max_thrust = 80. self.min_thrust = 25. self.max_thrust_raw = percentageToThrust(self.max_thrust) self.min_thrust_raw = percentageToThrust(self.min_thrust) self.old_thurst_raw = 0 self.slew_limit = 45 self.slew_rate = 30 self.slew_limit_raw = percentageToThrust(self.slew_limit) self.slew_rate_raw = percentageToThrust(self.slew_rate) self.dynserver = None self.prev_cmd = None self.curr_cmd = None self.yaw_joy = True self.sub_joy = rospy.Subscriber("/joy", JoyMSG, self.new_joydata) self.sub_tf = tf.TransformListener() self.pub_tf = tf.TransformBroadcaster() self.pub_cfJoy = rospy.Publisher("/cfjoy", CFJoyMSG, queue_size=2) self.pub_PID = rospy.Publisher("/pid", PidMSG,queue_size=50) # PIDs for R,P,Y,THROTTLE self.PID = (PID(), PID(), PID(), PID()) # XY, Throttle, Yaw self.PIDActive = (True, True, True, True) # XY, YAw, Throttle on/off self.PIDDelay = 0.0 self.PIDSource = Source.Qualisys self.PIDControl = True self.goal = [0.0, 0.0, 0.0, 0.0] # X Y Z YAW self.prev_msg = None self.PIDSetCurrentAuto = True self.thrust = (40., 90., 66.) # Min, Max, Base thrust self.distFunc = fLIN self.wandControl = False # Dynserver self.dynserver = DynamicReconfigureServer(CFJoyCFG, self.reconfigure)
def __init__(self, forearm=True): stereo_camera_names = [ "narrow_stereo", "wide_stereo" ] # narrow must be first as it can be alternate, and hence has more period restrictions. forearm_camera_names = ["forearm_r", "forearm_l"] self.camera_names = stereo_camera_names if forearm: self.camera_names = self.camera_names + forearm_camera_names # Parameter names are pretty symmetric. Build them up automatically. parameters = [param_rate, param_trig_mode] camera_parameters = dict( (name, dict((suffix, name + "_" + suffix) for suffix in parameters)) for name in self.camera_names) # Some parameters are common to the wide and narrow cameras. Also store # the node names. for camera in stereo_camera_names: camera_parameters[camera][param_rate] = "stereo_rate" camera_parameters[camera]["node_name"] = camera + "_both" if forearm: for camera in forearm_camera_names: camera_parameters[camera][ "node_name"] = camera[-1] + "_forearm_cam" for i in range(0, len(self.camera_names)): camera_parameters[self.camera_names[i]]["level"] = 1 << i # This only works because of the specific levels in the .cfg file. self.projector = Projector() self.cameras = dict( (name, Camera(proj=self.projector, **camera_parameters[name])) for name in self.camera_names) self.prosilica_inhibit = ProsilicaInhibitTriggerController( 'prosilica_inhibit_projector_controller', "prosilica_projector_inhibit", 0x0A, 0x00) self.controllers = [ ProjectorTriggerController('projector_trigger', self.projector), DualCameraTriggerController( 'head_camera_trigger', *[self.cameras[name] for name in stereo_camera_names]) ] if forearm: self.controllers = self.controllers + [ SingleCameraTriggerController('r_forearm_cam_trigger', self.cameras["forearm_r"]), SingleCameraTriggerController('l_forearm_cam_trigger', self.cameras["forearm_l"]), ] self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
def __init__(self): self.server = DynamicReconfigureServer(ConfigType, self.reconfigure) # Create a publisher for our custom message. pub = rospy.Publisher('chatter', String, queue_size=10) while not rospy.is_shutdown(): hello_str = "hello world %s" % rospy.get_time() rospy.loginfo(hello_str) rospy.loginfo('rate = %d', self.rate) rate = self.rate pub.publish(hello_str) r = rospy.Rate(rate) r.sleep()
def __init__(self): rate = rospy.get_param("~rate", 1.0) self.enable = True self.server = DynamicReconfigureServer(docConfig, self.reconfigure_cb) self.pub = rospy.Publisher("xavier_health", healthReportData, queue_size=10) self.enable = rospy.get_param("~enable", True) if self.enable: self.start() else: self.stop() rospy.Timer(rospy.Duration(1.0 / rate), self.timed_cb)
def __init__(self): """Read in parameters""" #obtenes los paranmetros del espacio de nombres privado desde el parameter serveer: rate = rospy.get_param('rate', 1.0) self.enable = True self.server = DynamicReconfigureServer(ConfigType, self.reconfigure_cb) self.pub = rospy.Publisher('example', NodeExampleData, queue_size=10) self.enable = rospy.get_param('enable', True) self.int_a = rospy.get_param('a', 1) self.int_b = rospy.get_param('b', 2) self.message = rospy.get_param('message', 'hello') if self.enable: self.start() else: self.stop() rospy.Timer(rospy.Duration(1 / rate), self.timer_cb)
def __init__(self): """Read in parameters.""" # Get the private namespace parameters from the parameter server: # set from either command line or launch file. self.sat_x = rospy.get_param('~sat_x', 1.0) self.sat_rot = rospy.get_param('~sat_rot', 1.0) self.vel_command = Twist() # Create a dynamic reconfigure server. self.server = DynamicReconfigureServer(ConfigType, self.reconfigure_cb) # Create a publisher for our custom message. self.pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=25) self.sub = rospy.Subscriber("vel_cmd", Point, self.callback) # Initialize message variables. # Create a timer to go to a callback at a specified interval. self.rate = rospy.Rate(10)
def __init__(self): self.rate = 10.0 self.enable = True self.server = DynamicReconfigureServer(ConfigType, self.reconfigure_cb) self.pub = rospy.Publisher('pychatter', SuperAwesome, queue_size=10) self.rate = rospy.get_param('~rate', 10.0) self.enable = rospy.get_param('~enable', True) self.message = rospy.get_param('~message', 'hello world') if self.enable: self.start() else: self.stop() rospy.Timer(rospy.Duration(1.0 / self.rate), self.timer_cb)
def __init__(self): pub = rospy.Publisher('/SuperAwesomeTopic', SuperAwesome, queue_size=1000) #Custom message publisher definition pub_ = rospy.Publisher('/DesiredRate', Int32, queue_size=1000)#Real rate message publisher definition rospy.init_node('py_publisher', anonymous=True) self.server = DynamicReconfigureServer(dynparamConfig, self.callback) #Dynamic reconfiguration definition while not rospy.is_shutdown(): rate = rospy.Rate(self.loop_rate) #Modifiable loop rate hello_str = "hello world" rospy.loginfo('DESIRED RATE %d' % (self.loop_rate)) pub.publish(hello_str) #Custom message publication pub_.publish(self.loop_rate) #Real Rate message publication rate.sleep()
def __init__(self, options): self.options = options self.preconfig = None # Advertise publishers self.pub_synth = rospy.Publisher("/synthetic/frame", synthMSG) self.pub_image = rospy.Publisher("/synthetic/image_raw", ImageMSG) self.pub_caminfo = rospy.Publisher("/synthetic/camera_info", CameraInfoMSG) self.pub_marker = rospy.Publisher("/synthetic/world_points", MarkerMSG) self.pub_tf = tf.TransformBroadcaster() self.bridge = CvBridge() # Subscribers self.sub_tf = tf.TransformListener() self.TCAM = [] self.TOBJ = [] self.OBJ2CAM = [] self.CAM2IMG = [] self.IMAGES = [] self.KPS = [] self.CLOUD = [] self.forwards = 1 self.cloudMarker = MarkerArrayMSG() self.load_data(options.path) self.step = 0 self.pause = False self.playRate = float(self.options.rate) self.rosRate = rospy.Rate(self.playRate) self.stepSize = int(self.options.stepSize) self.nr = len(self.IMAGES) self.pxNoise = 0.0 self.matchOutlier = 0.0 rospy.loginfo("Loaded %d images", self.nr) # Dynserver self.dynserver = DynamicReconfigureServer(synthCFG, self.reconfigure) while not rospy.is_shutdown(): self.sendSynth() self.rosRate.sleep()
def __init__(self): self.curve_movement = False self.axes_threshold = 0.5 self.max_speed = 0 self.key_items = { 'linear_axes': 'linear_axes', 'angular_axes': 'angular_axes', 'btn_turn_left': 'btn_turn_left', 'btn_turn_right': 'btn_turn_right', 'btn_speed_decr': 'btn_speed_decr', 'btn_speed_ecnr': 'btn_speed_ecnr', 'btn_front_flipper_up': 'btn_front_flipper_up', 'btn_front_flipper_down': 'btn_front_flipper_down', 'btn_rear_flipper_up': 'btn_rear_flipper_up', 'btn_rear_flipper_down': 'btn_rear_flipper_down', 'btn_both_flipper_up': 'btn_both_flipper_up', 'btn_both_flipper_down': 'btn_both_flipper_down', 'btn_epos_reset': 'btn_epos_reset', 'btn_front_led': 'btn_front_led', } self.angular_speed_factor = float( rospy.get_param('angular_speed_factor', '1.0')) self.linear_speed_factor = float( rospy.get_param('linear_speed_factor', '1.0')) self.enable = False rospy.Service("/mercury/trajectory/enable", SetEnabled, self._enable_service) self.joy = Joy(do_register=False) self.power_controller = PowerController() # connect to dynamic reconfigure server. DynamicReconfigureServer(TrajectoryConfig, self._configuration) self.movement_connection = MovementConnection( target=rospy.get_param('~board_ip', '192.168.10.170'), port=int(rospy.get_param('~board_port', '3020')), ) self.publisher = rospy.Publisher('/mercury/trajectory_raw', TrajectoryData, queue_size=10) self.commands = self.new_message() self.current_led_state = False self.power_controller.send('front_led', self.current_led_state)
def __init__(self): # Get the private namespace parameters from the parameter server: # set from either command line or launch file. rate = rospy.get_param('~rate', 1.0) rospy.loginfo('rate = %f', rate) # Create a dynamic reconfigure server. self.server = DynamicReconfigureServer(ConfigType, self.reconfigure_cb) # Create a publisher for our custom message. self.pub = rospy.Publisher('example', ECU, queue_size=10) # Initialize message variables. self.enable = rospy.get_param('~enable', True) self.int_motor = rospy.get_param('~motor', 90) self.int_servo = rospy.get_param('~servo', 90) # Create a timer to go to a callback at a specified interval. rospy.Timer(rospy.Duration(1 / rate), self.timer_cb) # Allow ROS to go to all callbacks. rospy.spin()
def __init__(self): # Get the private namespace parameters from command line or launch file. init_message = rospy.get_param('~message', 'hello') rate = float(rospy.get_param('~rate', '1.0')) rospy.loginfo('rate = %f', rate) # Create a dynamic reconfigure server. self.server = DynamicReconfigureServer(ConfigType, self.reconfigure_cb) # Create a publisher for our custom message. self.pub = rospy.Publisher('example', NodeExampleData, queue_size=10) # Initialize message variables. self.int_a = 1 self.int_b = 2 self.message = init_message # Create a timer to go to a callback. This is more accurate than # sleeping for a specified time. rospy.Timer(rospy.Duration(1 / rate), self.timer_cb) # Allow ROS to go to all callbacks. rospy.spin()
def __init__(self): # Parameters self.server = DynamicReconfigureServer(ConfigType, self.reconfigure_callback) self.numDisparities = self.convert_num_disparities( rospy.get_param('~numDisparities', 2)) self.blockSize = self.convert_block_size( rospy.get_param('~blockSize', 8)) # Stereo matcher and model self.block_matcher = cv2.StereoBM_create( numDisparities=self.numDisparities, blockSize=self.blockSize) self.model = StereoCameraModel() # TODO sg_block_matcher_ = cv::StereoSGBM::create(1, 1, 10); # Subscriber self.bridge = CvBridge() left_image_sub = message_filters.Subscriber( '/kitti/camera_gray_left/image_raw', Image) right_image_sub = message_filters.Subscriber( '/kitti/camera_gray_right/image_raw', Image) left_caminfo_sub = message_filters.Subscriber( '/kitti/camera_gray_left/camera_info', CameraInfo) right_caminfo_sub = message_filters.Subscriber( '/kitti/camera_gray_right/camera_info', CameraInfo) ts = message_filters.TimeSynchronizer([ left_image_sub, right_image_sub, left_caminfo_sub, right_caminfo_sub ], 10) ts.registerCallback(self.callback) # Publisher self.disparity_image_pub = rospy.Publisher("/kitti/disparity_image", Image, queue_size=10) self.depth_image_pub = rospy.Publisher("/kitti/depth_image", Image, queue_size=10) self.stereo_pointcloud_pub = rospy.Publisher( "/kitti/stereo_pointcloud", PointCloud, queue_size=10)
def __init__(self, translator, driverMgr): """img_interface_node constructor is meant to launch the dynamic reconfigure server until it is called to launch services for listening. It doesn't receive or return anything.""" self.translator = translator self.driverMgr = driverMgr self.avoidRemoteReconf = 0 try: ## Dynamic Reconfigure self.avoidRemoteReconf = self.avoidRemoteReconf + 1 # Launch self DynamicReconfigure server self.dynServer = DynamicReconfigureServer(PropertiesConfig, self.dynServerCallback) # Launch request listener services self.listenToRequests() rospy.loginfo("Interface for requests created.") except: rospy.logerr( "Error while trying to initialise dynamic parameter server.") raise return
def __init__(self): self.pub = rospy.Publisher( '/image/boxes', Boxes, queue_size=int(rospy.get_param("~queue_size", '10')) ) self.bridge = CvBridge() # TODO: make it reconfigureable self.kernel = np.ones((5, 5), np.uint8) self.min_area = 12 self.min_height = 96 self.min_width = 128 self.min_solidity = 0.5 self._image_sub = None DynamicReconfigureServer(DarkObjectsConfig, self.configuration) rospy.Service( '/{}/set_enable'.format(rospy.get_name()), SetEnabled, self.set_enable_cb )
def __init__(self): # Get the ~private namespace parameters from command line or launch file. init_message = rospy.get_param('~data', 'hola') rate = float(rospy.get_param('~rate', '1.0')) topic = rospy.get_param('~topic', 'message') rospy.loginfo('rate = %d', rate) rospy.loginfo('topic = %s', topic) # Create a dynamic reconfigure server. self.server = DynamicReconfigureServer(ConfigType, self.reconfigure) # Create a publisher for our custom message. pub = rospy.Publisher(topic, SuperAwesome,10) # Set the message to publish as our custom message. msg = SuperAwesome() # Initialize message variables. msg.data = init_message count = 1 elapsed = 0 # Main while loop. while not rospy.is_shutdown(): start = time.time() # Fill in custom message variables with values from dynamic reconfigure server. msg.data = self.data # Publish our custom message. pub.publish(msg) # Sleep for a while before publishing new messages. Division is so rate != period. if rate: rospy.sleep(1/rate) else: rospy.sleep(1.0) end = time.time() elapsed += end - start print 'time ', elapsed/count ,' of ', count, 'loops' count += 1; if count == 101: break
def __init__(self): self.roll = rospy.get_param("~roll", 0.0) self.pitch = rospy.get_param("~pitch", 0.0) self.server = DynamicReconfigureServer(Params, self.reconfigure) #print "R: %f P: %f" % (roll, pitch) #rospy.Subscriber('/cur_tilt_angle', std_msgs.msg.Float64, handle_kinect_tilt) #rospy.spin() self.angle_0 = tf.transformations.quaternion_from_euler(0, 0, 0) self.angle_1 = tf.transformations.quaternion_from_euler( -1.57, 0, -1.57) self.angle_2 = tf.transformations.quaternion_from_euler( -self.roll, -self.pitch, 0) while not rospy.is_shutdown(): stamp = rospy.Time.now() + rospy.Duration(0.3) tf.TransformBroadcaster().sendTransform( (0, 0, 0.036), self.angle_0, stamp, "/openni_camera", "/kinect_rotated_base") tf.TransformBroadcaster().sendTransform( (0, -0.02, 0), self.angle_0, stamp, "/openni_depth_frame", "/openni_camera") tf.TransformBroadcaster().sendTransform( (0, -0.04, 0), self.angle_0, stamp, "/openni_rgb_frame", "/openni_camera") tf.TransformBroadcaster().sendTransform( (0, 0, 0), self.angle_1, stamp, "/openni_depth_optical_frame", "/openni_depth_frame") tf.TransformBroadcaster().sendTransform( (0, 0, 0), self.angle_1, stamp, "/openni_rgb_optical_frame", "/openni_rgb_frame") tf.TransformBroadcaster().sendTransform( (0, 0, 0), self.angle_2, stamp, "/kinect_rotated_base", "/kinect_base") tf.TransformBroadcaster().sendTransform( (-0.077, 0, 0.35), self.angle_0, stamp, "/kinect_base", "/base_link") rospy.sleep(0.2)
def __init__(self): self.lockobj = thread.allocate_lock() # variables self.reference_image_msg = None self.templates = {} # frame_id : ref_image,ser_frame,ser_rect self.clock = [rospy.Time.now()] # publisher self.reference_pub = rospy.Publisher("current_template", Image, queue_size=1) self.result_pub = rospy.Publisher("result", TransformStamped, queue_size=1) self.debug_pub = rospy.Publisher("debug_image", Image, queue_size=1) # subscriber self.reference_image_sub = rospy.Subscriber( rospy.resolve_name("reference"), Image, self.ref_image_callback, queue_size=1) self.search_image_sub = rospy.Subscriber(rospy.resolve_name("search"), Image, self.ser_image_callback, queue_size=1) self.reference_point_sub = rospy.Subscriber( rospy.resolve_name("set_reference_point"), PointStamped, self.set_reference_point_callback, queue_size=1) self.search_sub = rospy.Subscriber( rospy.resolve_name("set_search_rect"), Rect, self.set_search_callback, queue_size=1) self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
class ThesisMasterNode(object): def __init__(self): # __init__ is much like a c++ constructor function, implicitly called, and used to initialize things client = dynamic_reconfigure.client.Client("pid_controller", timeout=1, config_callback=callback) rospy.Subscriber("odom_pos", pos, self.pos_callback) rospy.Subscriber("goal", curr_goal, self.curr_goal_callback) rospy.Subscriber("image", Image, self.image_callback) # Init our ROS Subscribers self.sub_pose = rospy.Subscriber('odom', Odometry, self.odom_callback) # Init our ROS Config server self.server = DynamicReconfigureServer(GoalConfig, self.config_callback) # Create a subscriber with appropriate topic, custom message and name of callback function. pubx = rospy.Publisher("x_pos", int64, queue_size=1) puby = rospy.Publisher("y_pos", int64, queue_size=1) pubtheta = rospy.Publisher("theta", int64, queue_size=1) # By the time we get to the end of __init__, we might not even need a while loop. # If the only time we're "thinking" is when we get a new image, odometry, or goal, then we # can just use those functions to run our program. Think "event-based" programming self.odom = Odometry() mainparam() rospy.spin()
def __init__(self): MM_REV = 12 if MM_REV >= 12: # Read the current settings from the Mini Maxwell try: self.bnds = GetCurrentBands(mm2name) except: self.bnds = Bands() else: # MM_REV < 12 self.bnds = Bands() # Get parameter rospy.logwarn( '[usage] rosrun mini_maxwell ros_client.py _ip:=[mini_maxwell ip or hostname] _round_trimp:=[round trip(ms)] _rate_limit:=[rate limit(bps)]' ) self.mm2name = rospy.get_param('~ip', '192.168.0.5') self.round_trip = rospy.get_param('~round_trip', 100) #100ms self.rate_limit = rospy.get_param('~rate_limit', 100000) #100kbps self.band_number = 5 rospy.loginfo('mm2 hostname or ip = %s', self.mm2name) # Create a dynamic reconfigure server. self.server = DynamicReconfigureServer(ConfigType, self.reconfigure) rospy.spin()
def __init__(self): self.button_names = [] self.axes_names = [] self.axes_to_button = [] self.button_to_axes = [] self.last_button_to_axes_states = {} self.axes_threshold = 0.025 self.map_axes = [] self.first_run = { 2: True, 5: True, } self.axes_button_trigger = {} # connect to dynamic reconfigure server. DynamicReconfigureServer(JoyConfig, self.configuration) # subscribe to ROS-joy topic. rospy.Subscriber( rospy.get_param('~joy_topic', default='/joy'), ROSJoy, self.callback, queue_size=int(rospy.get_param('~joy_queue_size', default='10')), ) # mercury-joy publisher. self.publisher = rospy.Publisher( '/mercury/joy', MercuryJoy, queue_size=int(rospy.get_param('~mercury_queue_size', default='1'))) # register a callback for ros shutdown rospy.on_shutdown(self.shutdown) self.message = MercuryJoy()
def __init__(self, sim=False): super(Core, self).__init__(sim) self.initial_point = self.loc dsrv = DynamicReconfigureServer(RobotConfig, self.Callback)
def __init__(self, intended_goal_index = 0, dim=2, udp_ip="127.0.0.1", udp_recv_port=8025, udp_send_port = 6000, buffer_size=8192*4): RosProcessingComm.__init__(self, udp_ip=udp_ip, udp_recv_port=udp_recv_port, udp_send_port=udp_send_port, buffer_size=buffer_size) print "In constructor" if rospy.has_param('framerate'): self.frame_rate = rospy.get_param('framerate') else: self.frame_rate = 60.0 # self.frame_rate = 60.0 self.server = DynamicReconfigureServer(ConfigType, self.reconfigureParams) self.is_trial_on = False self.period = rospy.Duration(1.0/self.frame_rate) self.lock = threading.Lock() self.rate =rospy.Rate(self.frame_rate) self.dim = dim self.running = True self.runningCV = threading.Condition() self.num_goals = 2 self.goal_threshold = 10 self.signal_sparsity = 0.0 self.random_direction = 0.8 self.rand_vec_scale = 1.0 self.mu = [0]*self.dim self.cov = np.eye(self.dim) assert(self.num_goals > 0) self.intended_goal_index = intended_goal_index assert(self.intended_goal_index < self.num_goals) self.goal_positions = npa([[0]*self.dim]*self.num_goals, dtype= 'f') self.autonomy_control_pub = None self.autonomy_robot_pose_pub = None self.autonomy_goal_pose_pub = None self.initializePublishers() if rospy.has_param('max_cart_vel'): self._max_cart_vel = np.array(rospy.get_param('max_cart_vel')) else: self._max_cart_vel = np.ones(self.dim) rospy.logwarn('No rosparam for max_cart_vel found...Defaulting to max linear velocity of 50 cm/s and max rotational velocity of 50 degrees/s') self.autonomy_vel = CartVelCmd() _dim = [MultiArrayDimension()] _dim[0].label = 'cartesian_velocity' _dim[0].size = 2 _dim[0].stride = 2 self.autonomy_vel.velocity.layout.dim = _dim self.autonomy_vel.velocity.data = np.zeros(self.dim) self.autonomy_vel.header.stamp = rospy.Time.now() self.autonomy_vel.header.frame_id = 'autonomy_control' self.velocity_scale = 0.25 self.autonomy_robot_pose = np.zeros(self.dim) self.autonomy_robot_pose_msg = Point() self.getRobotPosition() self.autonomy_goal_pose_msg = Float32MultiArray() _dim = [MultiArrayDimension()]*2 _dim[0].label = 'Goal Number' _dim[0].size = self.num_goals _dim[0].stride = self.num_goals*self.dim _dim[1].label = 'Dimension' _dim[1].size = self.dim _dim[1].stride = self.dim self.autonomy_goal_pose_msg.layout.dim = _dim self.autonomy_goal_pose_msg.data = [list(x) for x in list(self.goal_positions)] self.autonomy_goal_pose_pub.publish(self.autonomy_goal_pose_msg) self.data = CartVelCmd() self._msg_dim = [MultiArrayDimension()] self._msg_dim[0].label = 'cartesian_velocity' self._msg_dim[0].size = 2 self._msg_dim[0].stride = 2 self.data.velocity.layout.dim = _dim self.data.velocity.data = np.zeros(self.dim) self.data.header.stamp = rospy.Time.now() self.data.header.frame_id = 'autonomy_control' self.filter_length = 10; self.filter_list = [[0]*self.dim] * self.filter_length rospy.Service("point_robot_autonomy_control/set_autonomy_goals", GoalPoses, self.set_autonomy_goals) rospy.Service("point_robot_autonomy_control/trigger_trial", SetBool, self.trigger_trial) self.send_thread = threading.Thread(target=self._publish_command, args=(self.period,)) self.send_thread.start() rospy.loginfo("END OF CONSTRUCTOR - point_robot_autonomy_control_node")