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")
