def __init__(self,options=None): self.options = options 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.hover = False 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) # 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, frame_id='enu', map_size=500, resolution=0.3, rate=1): self.frame_id = frame_id self.ogrids = {} self.odom = None # Some default values self.plow = True self.plow_factor = 0 self.ogrid_min_value = -1 self.draw_bounds = False self.resolution = resolution self.ogrid_timeout = 2 self.enforce_bounds = False self.enu_bounds = [[0, 0, 1], [0, 0, 1], [0, 0, 1], [0, 0, 1]] # Default to centering the ogrid position = np.array([-(map_size * resolution) / 2, -(map_size * resolution) / 2, 0]) quaternion = np.array([0, 0, 0, 1]) self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion) self.global_ogrid = self.create_grid((map_size, map_size)) def set_odom(msg): return setattr(self, 'odom', mil_tools.pose_to_numpy(msg.pose.pose)) rospy.Subscriber('/odom', Odometry, set_odom) self.publisher = rospy.Publisher('/ogrid_master', OccupancyGrid, queue_size=1) self.ogrid_server = Server(OgridConfig, self.dynamic_cb) self.dynam_client = Client("bounds_server", config_callback=self.bounds_cb) self.ogrid_server.update_configuration({'width': 500}) rospy.Service("/center_ogrid", Trigger, self.center_ogrid) rospy.Timer(rospy.Duration(1.0 / rate), self.publish)
def __init__(self): self.server = actionlib.SimpleActionServer('track_object', TrackObjectAction, self.execute, False) self.server.start() self.lower = None self.upper = None self.targetType = None self.srv = Server(ThresholdsConfig, self.updateThresholds) self.thresholds = {int(k):v for k,v in rospy.get_param("/vision_thresholds").items()} self.bridge = CvBridge() self.leftImage = np.zeros((1,1,3), np.uint8) self.leftModel = image_geometry.PinholeCameraModel() self.newLeft = False self.rightImage = np.zeros((1,1,3), np.uint8) self.rightModel = image_geometry.PinholeCameraModel() self.newRight = False self.downImage = np.zeros((1,1,3), np.uint8) self.downModel = image_geometry.PinholeCameraModel() self.newDown = False self.disparityImage = np.zeros((1,1,3), np.uint8) self.stereoModel = image_geometry.StereoCameraModel() self.newDisparity = False self.poleFinder = PoleFinder() self.gateFinder = GateFinder() self.diceFinder = DiceFinder() self.pathFinder = PathFinder() self.response = TrackObjectResult() self.downSub = rospy.Subscriber('/down_camera/image_color', Image, self.downwardsCallback) self.downInfoSub = rospy.Subscriber('/down_camera/info', CameraInfo, self.downInfoCallback) self.stereoSub = rospy.Subscriber('/stereo/disparity', Image, self.stereoCallback) #self.stereoInfoSub = rospy.Subscriber('/stereo/info', CameraInfo, self.stereoInfoCallback) self.leftSub = rospy.Subscriber('/stereo/left/image', WFOVImage, self.leftCallback) #self.leftInfoSub = rospy.Subscriber('/stereo/left/image', WFOVImage, self.leftInfoCallback) self.rightSub = rospy.Subscriber('/stereo/right/image', WFOVImage, self.rightCallback) #self.rightInfoSub = rospy.Subscriber('/stereo/right/image', WFOVImage, self.rightInfoCallback) self.bridge = CvBridge() self.image_pub = rospy.Publisher("/thresh_image", Image, queue_size=10)
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, 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 main(): rospy.init_node("dynamic_parameters_server", anonymous=False) server = Server(ParametersConfig, parameter_changed_callback) rospy.spin()
#!/usr/bin/env python # _*_ coding:utf-8 _*_ ''' Description: 人脸跟踪时arduino节点代码可以使用dynamic_reconfigure来动态更新 节点参数。 ''' import rospy from dynamic_reconfigure.server import Server from arduino_servo_connect.cfg import arduino_servoConfig def callback(config, level): rospy.loginfo( "pub_rate:{firmware_pub_rate},x_deg:{default_x_deg},y_deg:{default_y_deg},move_delay:{servo_move_delay}" .format(**config)) return config if __name__ == "__main__": rospy.init_node("arduino_dynamic_reconfig", anonymous=False) Server(arduino_servoConfig, callback) rospy.spin()
def __init__(self): self.bridge = CvBridge() self.map_frame_id = rospy.get_param('~map_frame_id', 'map') self.frame_id = rospy.get_param('~frame_id', 'base_link') self.object_frame_id = rospy.get_param('~object_frame_id', 'object') self.color_hue = rospy.get_param('~color_hue', 90) # 160=purple, 100=blue, 10=Orange self.color_range = rospy.get_param('~color_range', 90) self.color_saturation = rospy.get_param('~color_saturation', 50) self.color_value = rospy.get_param('~color_value', 50) self.border = rospy.get_param('~border', 10) self.config_srv = Server(BlobDetectorConfig, self.config_callback) self.max_speed = rospy.get_param('~max_speed', 0.5) self.num_debris = 1 self.objects_positions = [] self.object_already_stored = False self.stop = False self.rospack = rospkg.RosPack() self.rospack.list() self.path = self.rospack.get_path('racecar_control') + "/report/" if os.path.exists((self.path + "points.txt")): os.remove((self.path + "points.txt")) params = cv2.SimpleBlobDetector_Params() # see https://www.geeksforgeeks.org/find-circles-and-ellipses-in-an-image-using-opencv-python/ # https://docs.opencv.org/3.4/d0/d7a/classcv_1_1SimpleBlobDetector.html params.thresholdStep = 10 params.minThreshold = 50 params.maxThreshold = 220 params.minRepeatability = 2 params.minDistBetweenBlobs = 10 # Set Color filtering parameters params.filterByColor = False params.blobColor = 255 # Set Area filtering parameters params.filterByArea = True params.minArea = 10 params.maxArea = 5000000000 # Set Circularity filtering parameters params.filterByCircularity = True params.minCircularity = 0.3 # Set Convexity filtering parameters params.filterByConvexity = False params.minConvexity = 0.95 # Set inertia filtering parameters params.filterByInertia = False params.minInertiaRatio = 0.1 # Set color_range value self.color_range = 90 self.detector = cv2.SimpleBlobDetector_create(params) self.br = tf.TransformBroadcaster() self.listener = tf.TransformListener() self.image_pub = rospy.Publisher('image_detections', Image, queue_size=1) self.object_pub = rospy.Publisher('object_detected', String, queue_size=1) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.image_sub = message_filters.Subscriber('image', Image) self.depth_sub = message_filters.Subscriber('depth', Image) self.info_sub = message_filters.Subscriber('camera_info', CameraInfo) self.ts = message_filters.TimeSynchronizer( [self.image_sub, self.depth_sub, self.info_sub], 10) self.ts.registerCallback(self.image_callback)
start_time = time.time() handle_scan(scan_message, delta_time) callback_time = time.time() - start_time print('callback_time:', callback_time) last_scan = scan_time def dynamic_configuration_callback(config, level): global parameters parameters = Parameters(config) pid.p = parameters.controller_p pid.i = parameters.controller_i pid.d = parameters.controller_d return config rospy.init_node('wallfollowing', anonymous=True) parameters = None pid = PIDController(1, 1, 1) rospy.Subscriber(TOPIC_LASER_SCAN, LaserScan, laser_callback) drive_parameters_publisher = rospy.Publisher(TOPIC_DRIVE_PARAMETERS, drive_param, queue_size=1) Server(wallfollowing2Config, dynamic_configuration_callback) while not rospy.is_shutdown(): rospy.spin()
class JoyController: def __init__(self,options=None): self.options = options 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.hover = False 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) # Dynserver self.dynserver = DynamicReconfigureServer(CFJoyCFG, self.reconfigure) def released(self, id): return self.prev_cmd.buttons[id] and not self.curr_cmd.buttons[id] def pressed(self, id): return not self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id] def held(self, id): return self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id] #Needed so the class can change the dynserver values with button presses def set_dynserver(self, server): self.dynserver = server def thurstToRaw(self, joy_thrust): # Deadman button or invalid thrust if not self.curr_cmd.buttons[Button.L1] or joy_thrust>1: return 0 raw_thrust = 0 if joy_thrust > 0.01: raw_thrust = self.min_thrust_raw + joy_thrust*(self.max_thrust_raw-self.min_thrust_raw) if self.slew_rate_raw>0 and self.slew_limit_raw > raw_thrust: if self.old_thurst_raw > self.slew_limit_raw: self.old_thurst_raw = self.slew_limit_raw if raw_thrust < (self.old_thurst_raw - (self.slew_rate_raw/100)): raw_thrust = self.old_thurst_raw - self.slew_rate_raw/100 if joy_thrust < 0 or raw_thrust < self.min_thrust_raw: raw_thrust = 0 self.old_thurst_raw = raw_thrust return raw_thrust def new_joydata(self, joymsg): global Button global Axes #Should run at 100hz. Use launch files roslaunch crazyflie_row joy.launch if self.prev_cmd == None: self.prev_cmd = joymsg print len(joymsg.axes) #USB mode if len(joymsg.axes) == 27: Button = enum(L1=10,R1=11,Select=0,Start=3,L2=8,R2=9,Up=4,Right=5,Down=6,Left=7,Square=15,Cross=14,Triangle=12,Circle=13) Axes = enum(SLL=0,SLU=1,SRL=2,SRU=3,Up=4,Right=5,Down=6,Left=7,L2=4+8,R2=4+9,L1=4+10,R1=4+11,Triangle=4+12,Circle=4+13,Cross=4+14,Square=4+15,AccL=16,AccF=17,AccU=18,GyroY=19) return (0,0,0,0,False,False,0) self.curr_cmd = joymsg hover = False # print "AXES" # for i,x in enumerate(joymsg.axes): # print " ",i,":",x # print "BUTTONS # for i,x in enumerate(joymsg.buttons): # print " ",i,":",x x = 0 y = 0 z = 0 r = 0 r2 = 0 # Get stick positions [-1 1] if self.curr_cmd.buttons[Button.L1]: x = self.joy_scale[0] * self.curr_cmd.axes[Axes.SLL] # Roll y = self.joy_scale[1] * self.curr_cmd.axes[Axes.SLU] # Pitch r = self.joy_scale[2] * self.curr_cmd.axes[Axes.SRL] # Yaw z = self.joy_scale[3] * self.curr_cmd.axes[Axes.SRU] # Thrust r2 = self.joy_scale[4] * (self.curr_cmd.axes[Axes.L2] - self.curr_cmd.axes[Axes.R2]) hover = self.curr_cmd.axes[Axes.L1]<-0.75 roll = x * self.max_angle pitch = y * self.max_angle yaw = 0 #TODO use dyn reconf to decide which to use if r2!=0: yaw = r2 * self.max_yawangle else: # Deadzone if r < -0.2 or r > 0.2: if r < 0: yaw = (r + 0.2) * self.max_yawangle * 1.25 else: yaw = (r - 0.2) * self.max_yawangle * 1.25 thrust = self.thurstToRaw(z) trimmed_roll = roll + self.trim_roll trimmed_pitch = pitch + self.trim_pitch # Control trim manually new_settings = {} if self.curr_cmd.buttons[Button.Left]: new_settings["trim_roll"] = max(self.trim_roll + self.curr_cmd.axes[Axes.Left]/10.0, -10) if self.curr_cmd.buttons[Button.Right]: new_settings["trim_roll"] = min(self.trim_roll - self.curr_cmd.axes[Axes.Right]/10.0, 10) if self.curr_cmd.buttons[Button.Down]: new_settings["trim_pitch"] = max(self.trim_pitch + self.curr_cmd.axes[Axes.Down]/10.0, -10) if self.curr_cmd.buttons[Button.Up]: new_settings["trim_pitch"] = min(self.trim_pitch - self.curr_cmd.axes[Axes.Up]/10.0, 10) # Set trim to current input if self.released(Button.R1): new_settings["trim_roll"] = min(10, max(trimmed_roll, -10)) new_settings["trim_pitch"] = min(10, max(trimmed_pitch, -10)) rospy.loginfo("Trim updated Roll/Pitch: %r/%r", round(new_settings["trim_roll"],2), round(new_settings["trim_roll"],2)) # Reset Trim if self.released(Button.Square): new_settings["trim_roll"] = 0 new_settings["trim_pitch"] = 0 rospy.loginfo("Pitch reset to 0/0") if new_settings != {} and self.dynserver!=None: self.dynserver.update_configuration(new_settings) hover_set = hover and not self.hover self.hover = hover #(trimmed_roll,trimmed_pitch,yaw,thrust,hover,hover_set, z) msg = CFJoyMSG() msg.header.stamp = rospy.Time.now() msg.roll = trimmed_roll msg.pitch = trimmed_pitch msg.yaw = yaw msg.thrust = thrust msg.hover = hover msg.hover_set = hover_set msg.hover_change = z msg.calib = self.pressed(Button.Triangle) self.pub_cfJoy.publish(msg) # Cache prev joy command self.prev_cmd = self.curr_cmd def reconfigure(self, config, level): self.trim_roll = config["trim_roll"] self.trim_pitch = config["trim_pitch"] self.max_angle = config["max_angle"] self.max_yawangle = config["max_yawangle"] self.max_thrust = config["max_thrust"] self.min_thrust = config["min_thrust"] self.slew_limit = config["slew_limit"] self.slew_rate = config["slew_rate"] self.max_thrust_yaw = percentageToThrust(self.max_thrust) self.min_thrust_yaw = percentageToThrust(self.min_thrust) self.slew_limit_yaw = percentageToThrust(self.slew_limit) self.slew_rate_yaw = percentageToThrust(self.slew_rate) return config
def __init__(self): rospy.wait_for_service('/performances/reload_properties') # States for wholeshow states = [{ 'name': 'sleeping', 'children': ['shutting'] }, { 'name': 'interacting', 'children': ['nonverbal'] }, 'performing', 'opencog', 'analysis'] HierarchicalMachine.__init__(self, states=states, initial='interacting') # Transitions self.add_transition('wake_up', 'sleeping', 'interacting') # Transitions self.add_transition('perform', 'interacting', 'performing') self.add_transition('interact', 'performing', 'interacting') self.add_transition('start_opencog', 'interacting', 'opencog') self.add_transition('shut', 'sleeping', 'sleeping_shutting') self.add_transition('be_quiet', 'interacting', 'interacting_nonverbal') self.add_transition('start_talking', 'interacting_nonverbal', 'interacting') # States handling self.on_enter_sleeping("start_sleeping") self.on_exit_sleeping("stop_sleeping") self.on_enter_interacting("start_interacting") self.on_exit_interacting("stop_interacting") self.on_enter_sleeping_shutting("system_shutdown") # ROS Handling rospy.init_node('WholeShow') self.btree_pub = rospy.Publisher("/behavior_switch", String, queue_size=5) self.btree_sub = rospy.Subscriber("/behavior_switch", String, self.btree_cb) self.soma_pub = rospy.Publisher('/blender_api/set_soma_state', SomaState, queue_size=10) self.look_pub = rospy.Publisher('/blender_api/set_face_target', Target, queue_size=10) self.gaze_pub = rospy.Publisher('/blender_api/set_gaze_target', Target, queue_size=10) self.performance_runner = rospy.ServiceProxy( '/performances/run_full_performance', srv.RunByName) # Wholeshow starts with behavior enabled, unless set otherwise rospy.set_param("/behavior_enabled", rospy.get_param("/behavior_enabled", True)) # Start sleeping. Wait for Blender to load rospy.wait_for_service('/blender_api/set_param') rospy.wait_for_service('/performances/current') self.blender_param = rospy.ServiceProxy('/blender_api/set_param', SetParam) time.sleep(2) self.sleeping = rospy.get_param('start_sleeping', False) if self.sleeping: t = threading.Timer(1, self.to_sleeping) t.start() # Performance id as key and keyword array as value self.performances_keywords = {} # Parse on load. # TODO make sure we reload those once performances are saved. self.after_performance = False # Speech handler. Receives all speech input, and forwards to chatbot if its not a command input, # or chat is enabled self.speech_sub = rospy.Subscriber('speech', ChatMessage, self.speech_cb) self.speech_pub = rospy.Publisher('chatbot_speech', ChatMessage, queue_size=10) # Sleep self.performance_events = rospy.Subscriber('/performances/events', Event, self.performances_cb) # Dynamic reconfigure self.config = {} self.cfg_srv = Server(WholeshowConfig, self.config_cb) # Behavior was paused entering into state self.behavior_paused = False # Chatbot was paused entering the state self.chatbot_paused = False self.sleeping = rospy.get_param('start_sleeping', False) # Preferred speech source self.speech_provider = rospy.get_param("active_stt", 'cloudspeech') self.speech_provider_active = False rospy.Subscriber('{}/status'.format(self.speech_provider), Bool, self.stt_status_cb)
rospy.loginfo("Publishing to %s" % (node.publisher.name)) node.ypubdebug = rospy.Publisher("yaw_pid_debug", Diagnose, queue_size=10) node.vpubdebug = rospy.Publisher("vel_pid_debug", Diagnose, queue_size=10) node.ydebugmsg = Diagnose() node.vdebugmsg = Diagnose() # Setup service #s = rospy.Service('set_engaged',SetBool,node.set_engaged_callback) s = rospy.Service('toggle_engaged', Empty, node.toggle_engaged_callback) # Setup subscribers s1 = rospy.Subscriber('odometry/nav', Odometry, node.odom_callback) rospy.loginfo("Subscribing to %s" % (s1.name)) if (yaw_type == 'yaw_rate'): s2 = rospy.Subscriber("cmd_vel", Twist, node.twist_callback) elif (yaw_type == 'yaw'): s2 = rospy.Subscriber("cmd_course", Course, node.course_callback) else: rospy.logerror("Don't know what to listen to for yaw_type <%s>" % yaw_type) sys.exit() rospy.loginfo("Subscribing to setpoint commands at %s" % (s2.name)) # Dynamic configure srv = Server(TwistDynamicConfig, node.dynamic_callback) try: rospy.spin() except rospy.ROSInterruptException: pass
def __init__(self): self.hue_white_l = rospy.get_param("~detect/lane/white/hue_l", 90) self.hue_white_h = rospy.get_param("~detect/lane/white/hue_h", 180) self.saturation_white_l = rospy.get_param( "~detect/lane/white/saturation_l", 128) self.saturation_white_h = rospy.get_param( "~detect/lane/white/saturation_h", 255) self.lightness_white_l = rospy.get_param( "~detect/lane/white/lightness_l", 128) self.lightness_white_h = rospy.get_param( "~detect/lane/white/lightness_h", 255) self.hue_yellow_l = rospy.get_param("~detect/lane/yellow/hue_l", 90) self.hue_yellow_h = rospy.get_param("~detect/lane/yellow/hue_h", 180) self.saturation_yellow_l = rospy.get_param( "~detect/lane/yellow/saturation_l", 128) self.saturation_yellow_h = rospy.get_param( "~detect/lane/yellow/saturation_h", 255) self.lightness_yellow_l = rospy.get_param( "~detect/lane/yellow/lightness_l", 128) self.lightness_yellow_h = rospy.get_param( "~detect/lane/yellow/lightness_h", 255) self.is_calibration_mode = rospy.get_param( "~is_detection_calibration_mode", False) if self.is_calibration_mode == True: srv_detect_lane = Server(DetectLaneParamsConfig, self.cbGetDetectLaneParam) self.sub_image_type = "raw" # you can choose image type "compressed", "raw" self.pub_image_type = "compressed" # you can choose image type "compressed", "raw" if self.sub_image_type == "compressed": # subscribes compressed image self.sub_image_original = rospy.Subscriber( '/detect/image_input/compressed', CompressedImage, self.cbFindLane, queue_size=1) elif self.sub_image_type == "raw": # subscribes raw image self.sub_image_original = rospy.Subscriber('/detect/image_input', Image, self.cbFindLane, queue_size=1) if self.pub_image_type == "compressed": # publishes lane image in compressed type self.pub_image_lane = rospy.Publisher( '/detect/image_output/compressed', CompressedImage, queue_size=1) elif self.pub_image_type == "raw": # publishes lane image in raw type self.pub_image_lane = rospy.Publisher('/detect/image_output', Image, queue_size=1) if self.is_calibration_mode == True: if self.pub_image_type == "compressed": # publishes lane image in compressed type self.pub_image_white_lane = rospy.Publisher( '/detect/image_output_sub1/compressed', CompressedImage, queue_size=1) self.pub_image_yellow_lane = rospy.Publisher( '/detect/image_output_sub2/compressed', CompressedImage, queue_size=1) elif self.pub_image_type == "raw": # publishes lane image in raw type self.pub_image_white_lane = rospy.Publisher( '/detect/image_output_sub1', Image, queue_size=1) self.pub_image_yellow_lane = rospy.Publisher( '/detect/image_output_sub2', Image, queue_size=1) self.pub_lane = rospy.Publisher('/detect/lane', Float64, queue_size=1) # subscribes state : yellow line reliability self.pub_yellow_line_reliability = rospy.Publisher( '/detect/yellow_line_reliability', UInt8, queue_size=1) # subscribes state : white line reliability self.pub_white_line_reliability = rospy.Publisher( '/detect/white_line_reliability', UInt8, queue_size=1) self.cvBridge = CvBridge() self.counter = 1 self.window_width = 1000. self.window_height = 600. self.reliability_white_line = 100 self.reliability_yellow_line = 100
def __init__(self): self.hue_red_l = rospy.get_param("~detect/lane/red/hue_l", 0) self.hue_red_h = rospy.get_param("~detect/lane/red/hue_h", 10) self.saturation_red_l = rospy.get_param( "~detect/lane/red/saturation_l", 30) self.saturation_red_h = rospy.get_param( "~detect/lane/red/saturation_h", 255) self.lightness_red_l = rospy.get_param("~detect/lane/red/lightness_l", 48) self.lightness_red_h = rospy.get_param("~detect/lane/red/lightness_h", 255) self.hue_yellow_l = rospy.get_param("~detect/lane/yellow/hue_l", 20) self.hue_yellow_h = rospy.get_param("~detect/lane/yellow/hue_h", 35) self.saturation_yellow_l = rospy.get_param( "~detect/lane/yellow/saturation_l", 100) self.saturation_yellow_h = rospy.get_param( "~detect/lane/yellow/saturation_h", 255) self.lightness_yellow_l = rospy.get_param( "~detect/lane/yellow/lightness_l", 50) self.lightness_yellow_h = rospy.get_param( "~detect/lane/yellow/lightness_h", 255) self.hue_green_l = rospy.get_param("~detect/lane/green/hue_l", 46) self.hue_green_h = rospy.get_param("~detect/lane/green/hue_h", 76) self.saturation_green_l = rospy.get_param( "~detect/lane/green/saturation_l", 86) self.saturation_green_h = rospy.get_param( "~detect/lane/green/saturation_h", 255) self.lightness_green_l = rospy.get_param( "~detect/lane/green/lightness_l", 50) self.lightness_green_h = rospy.get_param( "~detect/lane/green/lightness_h", 255) self.is_calibration_mode = rospy.get_param( "~is_detection_calibration_mode", False) if self.is_calibration_mode == True: srv_detect_lane = Server(DetectTrafficLightParamsConfig, self.cbGetDetectTrafficLightParam) self.sub_image_type = "raw" # "compressed" / "raw" self.pub_image_type = "raw" # "compressed" / "raw" self.counter = 1 if self.sub_image_type == "compressed": # subscribes compressed image self.sub_image_original = rospy.Subscriber( '/detect/image_input/compressed', CompressedImage, self.cbGetImage, queue_size=1) elif self.sub_image_type == "raw": # subscribes raw image self.sub_image_original = rospy.Subscriber( '/camera/image_compensated', Image, self.cbGetImage, queue_size=1) if self.pub_image_type == "compressed": # publishes compensated image in compressed type self.pub_image_traffic_light = rospy.Publisher( '/detect/image_output/compressed', CompressedImage, queue_size=1) elif self.pub_image_type == "raw": # publishes compensated image in raw type self.pub_image_traffic_light = rospy.Publisher( '/detect/image_output', Image, queue_size=1) if self.is_calibration_mode == True: if self.pub_image_type == "compressed": # publishes light image in compressed type self.pub_image_red_light = rospy.Publisher( '/detect/image_output_sub1/compressed', CompressedImage, queue_size=1) self.pub_image_yellow_light = rospy.Publisher( '/detect/image_output_sub2/compressed', CompressedImage, queue_size=1) self.pub_image_green_light = rospy.Publisher( '/detect/image_output_sub3/compressed', CompressedImage, queue_size=1) elif self.pub_image_type == "raw": # publishes light image in raw type self.pub_image_red_light = rospy.Publisher( '/detect/image_output_sub1', Image, queue_size=1) self.pub_image_yellow_light = rospy.Publisher( '/detect/image_output_sub2', Image, queue_size=1) self.pub_image_green_light = rospy.Publisher( '/detect/image_output_sub3', Image, queue_size=1) self.sub_traffic_light_finished = rospy.Subscriber( '/control/traffic_light_finished', UInt8, self.cbTrafficLightFinished, queue_size=1) self.pub_traffic_light_return = rospy.Publisher( '/detect/traffic_light_stamped', UInt8, queue_size=1) self.pub_parking_start = rospy.Publisher( '/control/traffic_light_start', UInt8, queue_size=1) self.pub_max_vel = rospy.Publisher('/control/max_vel', Float64, queue_size=1) self.StepOfTrafficLight = Enum( 'StepOfTrafficLight', 'searching_traffic_light searching_green_light searching_yellow_light searching_red_light waiting_green_light pass_traffic_light' ) self.cvBridge = CvBridge() self.cv_image = None self.is_image_available = False self.is_traffic_light_finished = False self.green_count = 0 self.yellow_count = 0 self.red_count = 0 self.stop_count = 0 self.off_traffic = False rospy.sleep(1) loop_rate = rospy.Rate(10) while not rospy.is_shutdown(): if self.is_image_available == True: self.fnFindTrafficLight() loop_rate.sleep()
if erroradj > 0: rot_speed = ((kp * erroradj) + (kd * derivative) + min_speed ) # turning right is positive value else: rot_speed = ((kp * erroradj) + (kd * derivative) - min_speed) preverror = erroradj #--------CONTROLLER ENDS HERE----------------------------------------- # BUILD ARRAY message speedmsg = Float32MultiArray() speedmsg.data = [lin_speed, rot_speed] pub_speeds.publish(speedmsg) rate.sleep() # INITIALIZE PUBLISHERS & SUBSCRIBERS #sub_theta = rospy.Subscriber('theta_gyro', Float32MultiArray, Get_IMU_Data) # from IMU sub_imu = rospy.Subscriber('IMU_data', BOSCH_IMU_DATA, Get_IMU_Data) sub_theta_ref = rospy.Subscriber('ref_theta', Float64, Get_Reference) # from kbd or Xbox controller pub_speeds = rospy.Publisher('speed_commands', Float32MultiArray, queue_size=1) rospy.init_node('controller_heading', anonymous=False) rate = rospy.Rate(100) # 100 Hz sampling/publishing frequency if __name__ == '__main__': rospy.on_shutdown(cleanupOnExit) srv = Server(controllerConfig, rcf_callback) control() rospy.spin()
class VisionServer: def __init__(self): self.server = actionlib.SimpleActionServer('track_object', TrackObjectAction, self.execute, False) self.server.start() self.lower = None self.upper = None self.targetType = None self.srv = Server(ThresholdsConfig, self.updateThresholds) self.thresholds = {int(k):v for k,v in rospy.get_param("/vision_thresholds").items()} self.bridge = CvBridge() self.leftImage = np.zeros((1,1,3), np.uint8) self.leftModel = image_geometry.PinholeCameraModel() self.newLeft = False self.rightImage = np.zeros((1,1,3), np.uint8) self.rightModel = image_geometry.PinholeCameraModel() self.newRight = False self.downImage = np.zeros((1,1,3), np.uint8) self.downModel = image_geometry.PinholeCameraModel() self.newDown = False self.disparityImage = np.zeros((1,1,3), np.uint8) self.stereoModel = image_geometry.StereoCameraModel() self.newDisparity = False self.poleFinder = PoleFinder() self.gateFinder = GateFinder() self.diceFinder = DiceFinder() self.pathFinder = PathFinder() self.response = TrackObjectResult() self.downSub = rospy.Subscriber('/down_camera/image_color', Image, self.downwardsCallback) self.downInfoSub = rospy.Subscriber('/down_camera/info', CameraInfo, self.downInfoCallback) self.stereoSub = rospy.Subscriber('/stereo/disparity', Image, self.stereoCallback) #self.stereoInfoSub = rospy.Subscriber('/stereo/info', CameraInfo, self.stereoInfoCallback) self.leftSub = rospy.Subscriber('/stereo/left/image', WFOVImage, self.leftCallback) #self.leftInfoSub = rospy.Subscriber('/stereo/left/image', WFOVImage, self.leftInfoCallback) self.rightSub = rospy.Subscriber('/stereo/right/image', WFOVImage, self.rightCallback) #self.rightInfoSub = rospy.Subscriber('/stereo/right/image', WFOVImage, self.rightInfoCallback) self.bridge = CvBridge() self.image_pub = rospy.Publisher("/thresh_image", Image, queue_size=10) #self.thresholds = self.loadThresholds() def updateThresholds(self, config, level): self.lower = np.array([config["lowH"], config["lowS"], config["lowL"]],dtype=np.uint8) self.upper = np.array([config["upH"], config["upS"], config["upL"]],dtype=np.uint8) if self.targetType is not None: self.thresholds[self.targetType] = [self.lower.tolist(), self.upper.tolist()] rospy.set_param("/vision_thresholds/"+str(self.targetType), [self.lower.tolist(), self.upper.tolist()]) return config def setThresholds(self, lower, upper): self.lower = np.array(lower,dtype=np.uint8) self.upper = np.array(upper,dtype=np.uint8) self.srv.update_configuration({"lowH":lower[0], "lowS":lower[1], "lowV":lower[2], "upH":upper[0], "upS":upper[1], "upV":upper[2]}) def execute(self, goal): self.targetType = goal.objectType self.setThresholds(*self.thresholds[self.targetType]) self.feedback = TrackObjectFeedback() self.found = False self.running = True self.ok = True rightImageRect = np.zeros(self.rightImage.shape, self.rightImage.dtype) leftImageRect = np.zeros(self.leftImage.shape, self.leftImage.dtype) downImageRect = np.zeros(self.downImage.shape, self.downImage.dtype) r = rospy.Rate(60) while self.running and not rospy.is_shutdown(): if self.server.is_preempt_requested() or self.server.is_new_goal_available(): self.running = False continue r.sleep() if self.rightModel.width is not None and self.rightImage is not None: self.rightModel.rectifyImage(self.rightImage, rightImageRect) else: rospy.logwarn_throttle(1, "No right camera model") continue if self.leftModel.width is not None and self.leftImage is not None: self.leftModel.rectifyImage(self.leftImage, leftImageRect) else: rospy.logwarn_throttle(1, "No left camera model") continue #We need the left camera model for stuff if self.downModel.width is not None and self.downImage is not None: self.downModel.rectifyImage(self.downImage, downImageRect) else: rospy.logwarn_throttle(1, "No down camera model") continue #We need the down camera model for stuff if self.targetType == TrackObjectGoal.startGate: if self.newRight: self.feedback = self.gateFinder.process(leftImageRect, rightImageRect, self.disparityImage, self.leftModel, self.stereoModel, self.upper, self.lower) self.newRight = False elif self.targetType == TrackObjectGoal.pole: if self.newRight: self.feedback = self.poleFinder.process(leftImageRect, rightImageRect, self.disparityImage, self.leftModel, self.stereoModel, self.upper, self.lower) self.newRight = False elif self.targetType == TrackObjectGoal.dice: if self.newRight: self.feedback = self.diceFinder.process(leftImageRect, rightImageRect, self.disparityImage, self.leftModel, self.stereoModel, goal.diceNum, self.upper, self.lower) self.newRight = False elif self.targetType == TrackObjectGoal.path: if self.newDown: self.feedback = self.pathFinder.process(downImageRect, self.downModel, self.upper, self.lower) self.newDown = False else: self.ok = False break if self.feedback.found: self.server.publish_feedback(self.feedback) self.feedback.found = False self.response.found=True if not goal.servoing: self.running = False self.response.stoppedOk=self.ok self.server.set_succeeded(self.response) #TODO: Fix color thresholds def downwardsCallback(self, msg): try: self.downImage = self.bridge.imgmsg_to_cv2(msg, "bgr8") self.newDown = True except CvBridgeError as e: print(e) #self.downModel.rectifyImage(self.downImage, self.downImage) def stereoCallback(self, msg): try: self.disparityImage = self.bridge.imgmsg_to_cv2(msg, "bgr8") self.newDisparity = True except CvBridgeError as e: print(e) def leftCallback(self, msg): try: self.leftImage = self.bridge.imgmsg_to_cv2(msg.image, "bgr8") self.leftModel.fromCameraInfo(msg.info) self.newLeft = True except CvBridgeError as e: print(e) def rightCallback(self, msg): try: self.rightImage = self.bridge.imgmsg_to_cv2(msg.image, "bgr8") self.rightModel.fromCameraInfo(msg.info) self.newRight = True except CvBridgeError as e: print(e) def downInfoCallback(self, msg): self.downModel.fromCameraInfo(msg) def rightInfoCallback(self, msg): self.rightModel.fromCameraInfo(msg.info) def leftInfoCallback(self, msg): self.leftModel.fromCameraInfo(msg.info) def loadThresholds(self): #with open(os.path.dirname(__file__) + '/../thresholds.json') as data_file: #json_data = json.load(data_file) data = {} #for entry in json_data: # high = entry['high'] #low = entry['low'] #data[entry['color']] = vision_utils.Thresholds(upperThresh=(high['hue'],high['sat'],high['val']), lowerThresh=(low['hue'],low['sat'],low['val'])) return data
class JoyController: 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 setXYRGoal(self, msg): new_settings = {} new_settings["x"] = msg.pose.position.x new_settings["y"] = msg.pose.position.y new_settings["rz"] = degrees(tf.transformations.euler_from_quaternion( [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])[2]) os.system("beep -f 6000 -l 35&") print new_settings self.dynserver.update_configuration(new_settings) def released(self, id): return self.prev_cmd.buttons[id] and not self.curr_cmd.buttons[id] def pressed(self, id): return not self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id] def held(self, id): return self.prev_cmd.buttons[id] and self.curr_cmd.buttons[id] #Needed so the class can change the dynserver values with button presses def set_dynserver(self, server): self.dynserver = server def thurstToRaw(self, joy_thrust): # Deadman button or invalid thrust if not self.curr_cmd.buttons[Button.L1] or joy_thrust>1: return 0 raw_thrust = 0 if joy_thrust > 0.016: raw_thrust = self.min_thrust_raw + joy_thrust*(self.max_thrust_raw-self.min_thrust_raw) if self.slew_rate_raw>0 and self.slew_limit_raw > raw_thrust: if self.old_thurst_raw > self.slew_limit_raw: self.old_thurst_raw = self.slew_limit_raw if raw_thrust < (self.old_thurst_raw - (self.slew_rate_raw/100)): raw_thrust = self.old_thurst_raw - self.slew_rate_raw/100 if joy_thrust < 0 or raw_thrust < self.min_thrust_raw: raw_thrust = 0 self.old_thurst_raw = raw_thrust return raw_thrust def new_joydata(self, joymsg): global Button global Axes #Should run at 100hz. Use launch files roslaunch crazyflie_ros joy.launch if self.prev_cmd == None: self.prev_cmd = joymsg print len(joymsg.axes) #USB mode if len(joymsg.axes) == 27: rospy.logwarn("Experimental: Using USB mode. Axes/Buttons may be different") Button = enum(L1=10,R1=11,Select=0,Start=3,L2=8,R2=9,Up=4,Right=5,Down=6,Left=7,Square=15,Cross=14,Triangle=12,Circle=13) Axes = enum(SLL=0,SLU=1,SRL=2,SRU=3,Up=4+4,Right=5+4,Down=6+4,Left=7+4,L2=4+8,R2=4+9,L1=4+10,R1=4+11,Triangle=4+12,Circle=4+13,Cross=4+14,Square=4+15,AccL=16,AccF=17,AccU=18,GyroY=19) self.USB = True return # bp = [i for i in range(len(joymsg.buttons)) if joymsg.buttons[i]] # ap = [(i,round(joymsg.axes[i],2)) for i in range(len(joymsg.axes)) if abs(joymsg.axes[i])>0.51 ] # if len(bp)>0: # print "Buttons:", bp # if len(ap)>0: # print "Axes :", ap if self.USB: # USB buttons go from 1 to -1, Bluetooth go from 0 to -1, so normalise joymsg.axes =np.array(joymsg.axes, dtype=np.float32) joymsg.axes[8:19] -=1 joymsg.axes[8:19] /=2 # Strange bug: left button as axis doesnt work. So use button instead joymsg.axes[11] = -joymsg.buttons[Button.Left]/2. self.curr_cmd = joymsg hover = False x = 0 y = 0 z = 0 r = 0 r2 = 0 # Get stick positions [-1 1] if self.curr_cmd.buttons[Button.L1]: # Deadman pressed (=allow flight) x = self.joy_scale[0] * self.curr_cmd.axes[Axes.SLL] # Roll y = self.joy_scale[1] * self.curr_cmd.axes[Axes.SLU] # Pitch r = self.joy_scale[2] * self.curr_cmd.axes[Axes.SRL] # Yaw z = self.joy_scale[3] * self.curr_cmd.axes[Axes.SRU] # Thrust r2 = self.joy_scale[4] * (self.curr_cmd.axes[Axes.L2] - self.curr_cmd.axes[Axes.R2]) hover = self.curr_cmd.axes[Axes.L1]<-0.75 roll = x * self.max_angle pitch = y * self.max_angle yaw = 0 #TODO use dyn reconf to decide which to use if r2!=0: yaw = r2 * self.max_yawangle else: if self.yaw_joy: # Deadzone if r < -0.2 or r > 0.2: if r < 0: yaw = (r + 0.2) * self.max_yawangle * 1.25 else: yaw = (r - 0.2) * self.max_yawangle * 1.25 if hover: thrust = int(round(deadband(z,0.2)*32767 + 32767)) #Convert to uint16 else: thrust = self.thurstToRaw(z) trimmed_roll = roll + self.trim_roll trimmed_pitch = pitch + self.trim_pitch # Control trim manually new_settings = {} if joymsg.header.seq%10 == 0: if self.curr_cmd.buttons[Button.Left]: new_settings["trim_roll"] = max(self.trim_roll + self.curr_cmd.axes[Axes.Left], -20) if self.curr_cmd.buttons[Button.Right]: new_settings["trim_roll"] = min(self.trim_roll - self.curr_cmd.axes[Axes.Right], 20) if self.curr_cmd.buttons[Button.Down]: new_settings["trim_pitch"] = max(self.trim_pitch + self.curr_cmd.axes[Axes.Down], -20) if self.curr_cmd.buttons[Button.Up]: new_settings["trim_pitch"] = min(self.trim_pitch - self.curr_cmd.axes[Axes.Up], 20) msg = CFJoyMSG() msg.header.stamp = rospy.Time.now() msg.roll = trimmed_roll msg.pitch = trimmed_pitch msg.yaw = yaw msg.thrust = thrust msg.hover = hover #msg.calib = self.pressed(Button.Triangle) # TODO: rotation is wrong! Cannot simply decompose x,y into r,p. # TODO: roll, pitch should be dependent from the distance and the angle. if self.PIDControl: # and hover: try: rtime = rospy.Time.now() # Update goal position if hover and joymsg.header.seq%10 == 0: if self.PIDActive[0]: if abs(x)>0.016: new_settings["x"] = self.goal[0]-roll/70. if abs(y)>0.016: new_settings["y"] = self.goal[1]-pitch/70. if self.PIDActive[1]: if abs(z)>0.016: new_settings["z"] = self.goal[2]+z/5 if self.PIDActive[2]: if abs(yaw)>0: new_settings["rz"] = self.goal[3]-yaw/self.max_yawangle*20 if new_settings.has_key("x") or new_settings.has_key("y") or new_settings.has_key("z") or new_settings.has_key("rz"): os.system("beep -f 6000 -l 15&") # Get error from current position to goal if possible [px,py,alt,rz] = self.getErrorToGoal() # Starting control mode if hover and not self.prev_msg.hover: # Starting Goal should be from current position if self.PIDSetCurrentAuto: new_settings["SetCurrent"] = True # Reset PID controllers for p in range(4): self.PID[p].reset(rtime) # Set yaw difference as I part #self.PID[3].error_sum = 0 if hover: msg.hover = False #TODO should be an option pidmsg = PidMSG() pidmsg.header.stamp = rtime # Get PID control #XY control depends on the asin of the distance #cr, crp, cri, crd = self.PID[0].get_command(-py,rtime,f=lambda d: degrees(asin(d))/90.*self.max_angle) #cp, cpp, cpi, cpd = self.PID[1].get_command(px,rtime,f=lambda d: degrees(asin(d))/90.*self.max_angle) #cr, crp, cri, crd = self.PID[0].get_command(-py,rtime) #cp, cpp, cpi, cpd = self.PID[1].get_command(px,rtime) # Distance to angle (X direction = pitch, Y direction = roll) cr, crp, cri, crd = self.PID[0].get_command(-py,rtime,f=self.distFunc ) cp, cpp, cpi, cpd = self.PID[1].get_command(px,rtime,f=self.distFunc ) # THRUST ct, ctp, cti, ctd = self.PID[2].get_command(alt,rtime) # YAW VEL cy, cyp, cyi, cyd = self.PID[3].get_command(-rz,rtime)#,d=rz,i=copysign(1,-rz)) #TODO: normalise rotation # ROll/Pitch if self.PIDActive[0]: msg.roll = cr + self.trim_roll msg.pitch = cp + self.trim_pitch pidmsg.ypid = [cr, crp, cri, -crd, -py] pidmsg.xpid = [cp, cpp, cpi, -cpd, px] # THRUST if self.PIDActive[1]: # Add base thrust ct += self.thrust[2] # Add roll compensation # TODO: done on the flie itself now @ 250hz # Bound ct = max(self.thrust[0],ct) ct = min(self.thrust[1],ct) # ct in thrust percent msg.thrust = percentageToThrust(ct) pidmsg.zpid= [ct, ctp, cti, ctd, alt] # YAW if self.PIDActive[2]: msg.yaw = cy pidmsg.rzpid= [cy, cyp, cyi, cyd, -rz] self.pub_PID.publish(pidmsg) if self.pressed(Button.Cross): new_settings["SetCurrent"] = True q = tf.transformations.quaternion_from_euler(radians(msg.roll-self.trim_roll), radians(msg.pitch - self.trim_pitch), radians(-msg.yaw)) self.pub_tf.sendTransform((0,0,0), q, rospy.Time.now(), "/cmd", "/cf_gt2d") except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): if time.time() - self.warntime>2: rospy.logwarn('Could not look up cf_gt2d -> goal') self.warntime = time.time() global tid if self.pressed(Button.Circle): tid = (tid +1)%len(trajectory) new_settings["x"], new_settings["y"], new_settings["z"], new_settings["rz"] = trajectory[tid] # previous if self.pressed(Button.Triangle): tid = (tid -1)%len(trajectory) new_settings["x"], new_settings["y"], new_settings["z"], new_settings["rz"] = trajectory[tid] # Set trim to current input if self.released(Button.R1): new_settings["trim_roll"] = min(20, max(msg.roll, -20)) new_settings["trim_pitch"] = min(20, max(msg.pitch, -20)) rospy.loginfo("Trim updated Roll/Pitch: %r/%r", round(new_settings["trim_roll"],2), round(new_settings["trim_pitch"],2)) # Reset Trim if self.released(Button.Square): new_settings["trim_roll"] = 0.0 new_settings["trim_pitch"] = 0.0 rospy.loginfo("Pitch reset to 0/0") if new_settings != {} and self.dynserver is not None: if self.USB: new_settings = numpy2python(new_settings) self.dynserver.update_configuration(new_settings) # Cache prev joy command self.pub_cfJoy.publish(msg) self.prev_cmd = self.curr_cmd self.prev_msg = msg self.prev_msg.hover = hover def lookupTransformInWorld(self, frame='/cf_gt', forceRealtime = False, poseNoiseMeters=0.0 ): now = rospy.Time(0) if self.PIDDelay > 0.00001 and not forceRealtime: #rospy.logwarn('Delay in TF Pose: %5.3s', self.PIDDelay) now = rospy.Time.now()-rospy.Duration(self.PIDDelay) (trans,rot) = self.sub_tf.lookupTransform('/world', frame, now) if poseNoiseMeters>0: trans = [trans[0]+random.normalvariate(0,poseNoiseMeters), trans[1]+random.normalvariate(0,poseNoiseMeters), trans[2]+random.normalvariate(0,poseNoiseMeters)] return (trans,rot) def getErrorToGoal(self): # Look up flie position, convert to 2d, publish, return error if self.wandControl: #update goal using wand try: (t,r) = self.lookupTransformInWorld(frame='/wand', forceRealtime=True) e = tf.transformations.euler_from_quaternion(r) self.goal= [t[0], t[1], t[2], degrees(e[2]-e[0])] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo('Could not find wand') # Publish GOAL from world frame q = tf.transformations.quaternion_from_euler(0, 0, radians(self.goal[3])) self.pub_tf.sendTransform((self.goal[0],self.goal[1],self.goal[2]), q, rospy.Time.now(), "/goal", "/world") # Look up 6D flie pose, publish 3d+yaw pose (trans,rot) = self.lookupTransformInWorld(frame='/cf_gt', poseNoiseMeters=0.0) euler = tf.transformations.euler_from_quaternion(rot) q = tf.transformations.quaternion_from_euler(0, 0, euler[2]) self.pub_tf.sendTransform(trans, q, rospy.Time.now(), "/cf_gt2d", "/world") # Look up error goal->cf (trans,rot) = self.sub_tf.lookupTransform('/cf_gt2d', '/goal', rospy.Time(0)) #self.pub_tf.sendTransform(trans, rot, rospy.Time.now(), "/goalCF", "/cf_gt2d") euler = tf.transformations.euler_from_quaternion(rot) return trans[0], trans[1], trans[2], degrees(euler[2]) def setGoalFromCurrent(self,config={}): try: (trans,rot) = self.lookupTransformInWorld(frame='/cf_gt', forceRealtime=True) euler = tf.transformations.euler_from_quaternion(rot) config['x'],config['y'],config['z'],config['rz']= trans[0], trans[1], trans[2], degrees(euler[2]) rospy.loginfo('Updated goal state: [%.2f %.2f %.2f %.1f]', trans[0], trans[1], trans[2], degrees(euler[2])) self.goal = [config['x'],config['y'],config['z'],config['rz']] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('Could not look up transform world -> cf_gt') return config def reconfigure(self, config, level): # Manual control stuff self.trim_roll = config["trim_roll"] self.trim_pitch = config["trim_pitch"] self.max_angle = config["max_angle"] self.max_yawangle = config["max_yawangle"] self.max_thrust = config["max_thrust"] self.min_thrust = config["min_thrust"] self.slew_limit = config["slew_limit"] self.slew_rate = config["slew_rate"] self.max_thrust_raw = percentageToThrust(self.max_thrust) self.min_thrust_raw = percentageToThrust(self.min_thrust) self.slew_limit_raw = percentageToThrust(self.slew_limit) self.slew_rate_raw = percentageToThrust(self.slew_rate) self.yaw_joy = config["yaw_joy"] #PID control stuff if config["PIDPreset"]>0: if config["PIDPreset"]==1: # Aggressive config["DistFunc"] = 1 # ATAN config["Pxy"] = 1.0 config["Ixy"] = 0.0 config["Dxy"] = 0.2 config["Pyaw"] = 8 config["Iyaw"] = 0.0 config["Dyaw"] = 0.0 config["Pz"] = 20 config["Iz"] = 5 config["Dz"] = 8 config["RP_maxAngle"] = 35 config["Y_maxVel"] = 250 config["z_minThrust"] = 50 config["z_maxThrust"] = 100 config["z_baseThrust"] = 73 config["Response"] = 0.3 if config["PIDPreset"]==2: # Passive config["DistFunc"] = 2 # ASIN config["Pxy"] = 2.0 config["Ixy"] = 0.0 config["Dxy"] = 0.4 config["Pyaw"] = 2.0 config["Iyaw"] = 0.0 config["Dyaw"] = 0.0 config["Pz"] = 20 config["Iz"] = 5 config["Dz"] = 8 config["RP_maxAngle"] = 7 config["Y_maxVel"] = 80 config["z_minThrust"] = 50 config["z_maxThrust"] = 90 config["z_baseThrust"] = 73 config["Response"] = 0.175 if config["PIDPreset"]==3: # Linear config["DistFunc"] = 0 config["Pxy"] = 17 config["Ixy"] = 0.0 config["Dxy"] = 5 config["Pyaw"] = 3.3 config["Iyaw"] = 0.0 config["Dyaw"] = 0.0 config["Pz"] = 20 config["Iz"] = 5 config["Dz"] = 8 config["RP_maxAngle"] = 25 config["Y_maxVel"] = 300 config["z_minThrust"] = 50 config["z_maxThrust"] = 90 config["z_baseThrust"] = 73 config["Response"] = 1 rospy.loginfo("Preset Loaded") config["PIDPreset"] = 0 # Settings self.PIDDelay = config["Delay"] self.PIDSource = config["Source"] self.PIDControl = config["Control"] self.max_anglePID = config['RP_maxAngle'] # Gains self.PID[0].set_gains(config["Pxy"], config["Ixy"], config["Dxy"] ) self.PID[1].set_gains(config["Pxy"], config["Ixy"], config["Dxy"] ) self.PID[2].set_gains(config["Pz"], config["Iz"], config["Dz"] ) self.PID[3].set_gains(config["Pyaw"], config["Iyaw"], config["Dyaw"] ) # Goal self.wandControl = config['WandControl'] if config['SetCurrent']: config['SetCurrent'] = False config = self.setGoalFromCurrent(config) if config['Set'] or config['LiveUpdate']: config['Set'] = False if not self.wandControl: self.goal = [config['x'],config['y'],config['z'],config['rz']] # Limits #self.limits = (config['RP_maxAngle'], config['Y_maxVel'], config['z_maxAcc']) self.PID[0].set_limits(config['RP_maxAngle']) self.PID[1].set_limits(config['RP_maxAngle']) self.PID[2].set_limits(100) self.PID[3].set_limits(config['Y_maxVel']) self.thrust = (config['z_minThrust'],config['z_maxThrust'], config['z_baseThrust'] ) # On/OFF self.PIDActive = (config['xyControl'],config['thrustControl'],config['yawControl']) self.PIDSetCurrentAuto = config['SetCurrentAuto'] self.distFunc = [lambda x: fLIN(x,config["Response"]), lambda x: fATAN(x,config["Response"]), lambda x: fASIN(x,config["Response"])][config['DistFunc']] return config
class VectorDriver: def __init__(self): """ Variables to track communication frequency for debugging """ self.summer=0 self.samp = 0 self.avg_freq = 0.0 self.start_frequency_samp = False self.need_to_terminate = False self.flush_rcvd_data=True self.update_base_local_planner = False self.parameter_server_is_updating = False self.last_move_base_update = rospy.Time.now().to_sec() """ Initialize the publishers for VECTOR """ self.vector_data = VECTOR_DATA() """ Start the thread for the linear actuator commands """ self._linear = LinearActuator() if (False == self._linear.init_success): rospy.logerr("Could not initialize the linear actuator interface! exiting...") return """ Initialize faultlog related items """ self.is_init = True self.extracting_faultlog = False """ Initialize the dynamic reconfigure server for VECTOR """ self.param_server_initialized = False self.dyn_reconfigure_srv = Server(vectorConfig, self._dyn_reconfig_callback) """ Wait for the parameter server to set the configs and then set the IP address from that. Note that this must be the current ethernet settings of the platform. If you want to change it set the ethernet settings at launch to the current ethernet settings, power up, change them, power down, set the the ethernet settings at launch to the new ones and relaunch """ r = rospy.Rate(10) start_time = rospy.Time.now().to_sec() while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == self.param_server_initialized): r.sleep() if (False == self.param_server_initialized): rospy.logerr("Parameter server not found, you must pass an initial yaml in the launch! exiting...") return """ Create the thread to run VECTOR communication """ self.tx_queue_ = multiprocessing.Queue() self.rx_queue_ = multiprocessing.Queue() self.comm = IoEthThread((os.environ['VECTOR_IP_ADDRESS'],int(os.environ['VECTOR_IP_PORT_NUM'])), self.tx_queue_, self.rx_queue_, max_packet_size=1248) if (False == self.comm.link_up): rospy.logerr("Could not open socket for VECTOR...") self.comm.close() return """ Initialize the publishers and subscribers for the node """ self.faultlog_pub = rospy.Publisher('/vector/feedback/faultlog', Faultlog, queue_size=10,latch=True) rospy.Subscriber("/vector/cmd_vel", Twist, self._add_motion_command_to_queue) rospy.Subscriber("/vector/gp_command",ConfigCmd,self._add_config_command_to_queue) rospy.Subscriber("/move_base/DWAPlannerROS/parameter_updates",Config,self._update_move_base_params) """ Start the receive handler thread """ self.terminate_mutex = threading.RLock() self.last_rsp_rcvd = rospy.Time.now().to_sec() self._rcv_thread = threading.Thread(target = self._run) self._rcv_thread.start() """ Start streaming continuous data """ rospy.loginfo("Stopping the data stream") if (False == self._continuous_data(False)): rospy.logerr("Could not stop VECTOR communication stream") self.Shutdown() return """ Extract the faultlog at startup """ self.flush_rcvd_data=False rospy.loginfo("Extracting the faultlog") self.extracting_faultlog = True if (False == self._extract_faultlog()): rospy.logerr("Could not get retrieve VECTOR faultlog") self.Shutdown() return """ Start streaming continuous data """ rospy.loginfo("Starting the data stream") if (False == self._continuous_data(True)): rospy.logerr("Could not start VECTOR communication stream") self.Shutdown() return self.start_frequency_samp = True """ Force the configuration to update the first time to ensure that the variables are set to the correct values on the machine """ if (False == self._force_machine_configuration(True)): rospy.logerr("Initial configuration parameteters my not be valid, please check them in the yaml file") rospy.logerr("The ethernet address must be set to the present address at startup, to change it:") rospy.logerr("start the machine; change the address using rqt_reconfigure; shutdown; update the yaml and restart") self.Shutdown() return """ Uncomment the line below to always unlock gain tuning and force the upload based on the parameters in the yaml file you must have the key to do this because it can be dangerous if one sets unstable gains. 0x00000000 needs to be replaced by the key """ #self._unlock_gain_tuning(0x00000000) #self._force_pid_configuration(True) """ Finally update the values for dynamic reconfigure with the ones reported by the machine """ new_config = dict({"x_vel_limit_mps" : self.vector_data.config_param.machcfg.x_vel_limit_mps, "y_vel_limit_mps" : self.vector_data.config_param.machcfg.y_vel_limit_mps, "accel_limit_mps2" : self.vector_data.config_param.machcfg.accel_limit_mps2, "dtz_decel_limit_mps2": self.vector_data.config_param.machcfg.dtz_decel_limit_mps2, "yaw_rate_limit_rps" : self.vector_data.config_param.machcfg.yaw_rate_limit_rps, "wheel_diameter_m": self.vector_data.config_param.machcfg.wheel_diameter_m, "wheel_base_length_m": self.vector_data.config_param.machcfg.wheelbase_length_m, "wheel_track_width_m": self.vector_data.config_param.machcfg.wheel_track_width_m, "gear_ratio" : self.vector_data.config_param.machcfg.gear_ratio, "motion_while_charging" : ((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1, "motion_ctl_input_filter" : (self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK, "p_gain_rps_per_rps" : self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps, "i_gain_rps_per_rad" : self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad, "d_gain_rps_per_rps2" : self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2, "fdfwd_gain_rps_per_motor_rps" : self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps, "p_error_limit_rps" : self.vector_data.config_param.ctlconfig.p_error_limit_rps, "i_error_limit_rad" : self.vector_data.config_param.ctlconfig.i_error_limit_rad, "i_error_drain_rate_rad_per_frame" : self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame, "input_target_limit_rps" : self.vector_data.config_param.ctlconfig.input_target_limit_rps, "output_limit_rps" : self.vector_data.config_param.ctlconfig.input_target_limit_rps}) self.dyn_reconfigure_srv.update_configuration(new_config) rospy.loginfo("Vector Driver is up and running") def Shutdown(self): with self.terminate_mutex: self.need_to_terminate = True rospy.loginfo("Vector Driver has called the Shutdown method, terminating") self._linear.Shutdown() self.comm.Close() self.tx_queue_.close() self.rx_queue_.close() def _run(self): while not self.need_to_terminate: """ Run until signaled to stop Perform the actions defined based on the flags passed out """ result = select.select([self.rx_queue_._reader],[],[],0.02) if len(result[0]) > 0: data = result[0][0].recv() with self.terminate_mutex: if not self.need_to_terminate: self._handle_rsp(data) def _add_command_to_queue(self,command): """ Create a byte array with the CRC from the command """ cmd_bytes = generate_cmd_bytes(command) """ Send it """ self.tx_queue_.put(cmd_bytes) def _update_rcv_frq(self): if (True == self.start_frequency_samp): self.samp+=1 self.summer+=1.0/(rospy.Time.now().to_sec() - self.last_rsp_rcvd) self.avg_freq = self.summer/self.samp self.last_rsp_rcvd = rospy.Time.now().to_sec() def _handle_rsp(self,data_bytes): self._update_rcv_frq() if (True == self.flush_rcvd_data): return valid_data,rsp_data = validate_response(data_bytes) if (False == valid_data): rospy.logerr("bad vector data packet") return if (self.extracting_faultlog): if (len(rsp_data) == NUMBER_OF_FAULTLOG_WORDS): self.extracting_faultlog = False faultlog_msg = Faultlog() faultlog_msg.data = rsp_data self.faultlog_pub.publish(faultlog_msg) elif (len(rsp_data) == NUMBER_OF_VECTOR_RSP_WORDS): header_stamp = self.vector_data.status.parse(rsp_data[START_STATUS_BLOCK:END_STATUS_BLOCK]) wheel_circum = self.vector_data.config_param.parse(rsp_data[START_APP_CONFIG_BLOCK:END_FRAM_CONFIG_BLOCK],header_stamp) self.vector_data.auxiliary_power.parse(rsp_data[START_BATTERY_DATA_BLOCK:END_BATTERY_DATA_BLOCK],header_stamp) self.vector_data.propulsion.parse(rsp_data[START_PROPULSION_DATA_BLOCK:END_PROPULSION_DATA_BLOCK],header_stamp) self.vector_data.dynamics.parse(rsp_data[START_DYNAMICS_DATA_BLOCK:END_DYNAMICS_DATA_BLOCK],header_stamp,wheel_circum) self.vector_data.imu.parse_data(rsp_data[START_IMU_DATA_BLOCK:END_IMU_DATA_BLOCK],header_stamp) rospy.logdebug("feedback received from vector") def _add_motion_command_to_queue(self,command): """ Add the command to the queue, platform does command limiting and mapping """ cmds = [MOTION_CMD_ID,[convert_float_to_u32(command.linear.x), convert_float_to_u32(command.linear.y), convert_float_to_u32(command.angular.z)]] if (False == self.parameter_server_is_updating): self._add_command_to_queue(cmds) def _add_config_command_to_queue(self,command): try: cmds = [GENERAL_PURPOSE_CMD_ID,[command_ids[command.gp_cmd],command.gp_param]] self._add_command_to_queue(cmds) except: rospy.logerr("Config param failed, it is probably not known") return def _dyn_reconfig_callback(self,config,level): """ Note: for some reason the commands collide (queue gets garbled) if the motion commands and reconfiguration are sent at the same time; so we just gate the motion commands when reconfiguring which should be OK in general since most result in zero velocity on the embedded side. Need to dig in further to understand why this is happening """ self.parameter_server_is_updating = True rospy.sleep(0.1) """ The first time through we want to ignore the values because they are just defaults from the ROS parameter server which has no knowledge of the platform being used """ if (True == self.is_init): self.is_init = False return config """ Create the configuration bitmap from the appropriate variables """ config_bitmap = (((config.motion_while_charging^1) << DISABLE_AC_PRESENT_CSI_SHIFT)| (config.motion_ctl_input_filter << MOTION_MAPPING_FILTER_SHIFT)) """ Define the configuration parameters for all the platforms """ self.valid_config_cmd = [LOAD_MACH_CONFIG_CMD_ID, [convert_float_to_u32(config.x_vel_limit_mps), convert_float_to_u32(config.y_vel_limit_mps), convert_float_to_u32(config.accel_limit_mps2), convert_float_to_u32(config.decel_limit_mps2), convert_float_to_u32(config.dtz_decel_limit_mps2), convert_float_to_u32(config.yaw_rate_limit_rps), convert_float_to_u32(config.yaw_accel_limit_rps2), convert_float_to_u32(config.wheel_diameter_m), convert_float_to_u32(config.wheel_base_length_m), convert_float_to_u32(config.wheel_track_width_m), convert_float_to_u32(config.gear_ratio), config_bitmap]] """ The teleop limits are always the minimum of the actual machine limit and the ones set for teleop """ config.teleop_x_vel_limit_mps = minimum_f(config.teleop_x_vel_limit_mps, config.x_vel_limit_mps) config.teleop_y_vel_limit_mps = minimum_f(config.teleop_y_vel_limit_mps, config.x_vel_limit_mps) config.teleop_accel_limit_mps2 = minimum_f(config.teleop_accel_limit_mps2, config.accel_limit_mps2) config.teleop_yaw_rate_limit_rps = minimum_f(config.teleop_yaw_rate_limit_rps, config.yaw_rate_limit_rps) config.teleop_yaw_accel_limit_rps2 = minimum_f(config.teleop_yaw_accel_limit_rps2, config.teleop_yaw_accel_limit_rps2) """ Set the teleop configuration in the feedback """ self.vector_data.config_param.SetTeleopConfig([config.teleop_x_vel_limit_mps, config.teleop_y_vel_limit_mps, config.teleop_accel_limit_mps2, config.teleop_yaw_rate_limit_rps, config.teleop_yaw_accel_limit_rps2]) """ Define the configuration parameters for all the platforms """ self.valid_pid_cmd = [SET_PID_CONFIG_CMD_ID, [convert_float_to_u32(config.p_gain_rps_per_rps), convert_float_to_u32(config.i_gain_rps_per_rad), convert_float_to_u32(config.d_gain_rps_per_rps2), convert_float_to_u32(config.fdfwd_gain_rps_per_motor_rps), convert_float_to_u32(config.p_error_limit_rps), convert_float_to_u32(config.i_error_limit_rad), convert_float_to_u32(config.d_error_limit_rps2), convert_float_to_u32(config.i_error_drain_rate_rad_per_frame), convert_float_to_u32(config.output_limit_rps), convert_float_to_u32(config.input_target_limit_rps)]] """ Update the linear actuator velocity limit """ """ Check and see if we need to store the parameters in NVM before we try, although the NVM is F-RAM with unlimited read/write, uneccessarily setting the parameters only introduces risk for error """ load_config_params = False if self.param_server_initialized: if ((1<<7) == ((1<<7) & level)): self._linear.UpdateVelLimit(config.linear_actuator_vel_limit_mps) if ((1<<2) == ((1<<2) & level)): self._force_machine_configuration() load_config_params = True config.x_vel_limit_mps = round(self.vector_data.config_param.machcfg.x_vel_limit_mps,3) config.y_vel_limit_mps = round(self.vector_data.config_param.machcfg.y_vel_limit_mps,3) config.accel_limit_mps2 = round(self.vector_data.config_param.machcfg.accel_limit_mps2,3) config.decel_limit_mps2 = round(self.vector_data.config_param.machcfg.decel_limit_mps2,3) config.dtz_decel_limit_mps2 = round(self.vector_data.config_param.machcfg.dtz_decel_limit_mps2,3) config.yaw_rate_limit_rps = round(self.vector_data.config_param.machcfg.yaw_rate_limit_rps,3) config.yaw_accel_limit_rps2 = round(self.vector_data.config_param.machcfg.yaw_accel_limit_rps2,3) config.wheel_diameter_m = round(self.vector_data.config_param.machcfg.wheel_diameter_m,3) config.wheel_base_length_m = round(self.vector_data.config_param.machcfg.wheelbase_length_m,3) config.wheel_track_width_m = round(self.vector_data.config_param.machcfg.wheel_track_width_m,3) config.gear_ratio = round(self.vector_data.config_param.machcfg.gear_ratio,3) config.motion_while_charging = ((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1 config.motion_ctl_input_filter = (self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK if self.param_server_initialized: if ((1<<5) == ((1<<5) & level)): if self.vector_data.config_param.ctlconfig.control_tuning_unlocked: self._force_pid_configuration() if (True == config.set_default_gains): cmds = [GENERAL_PURPOSE_CMD_ID,[2002,0]] self._add_command_to_queue(cmds) config.set_default_gains = False rospy.sleep(0.1) config.p_gain_rps_per_rps = round(self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps,3) config.i_gain_rps_per_rad = round(self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad,3) config.d_gain_rps_per_rps2 = round(self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2,3) config.fdfwd_gain_rps_per_motor_rps = round(self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps,3) config.p_error_limit_rps = round(self.vector_data.config_param.ctlconfig.p_error_limit_rps,3) config.i_error_limit_rad = round(self.vector_data.config_param.ctlconfig.i_error_limit_rad,3) config.d_error_limit_rps2 = round(self.vector_data.config_param.ctlconfig.d_error_limit_rps2,3) config.i_error_drain_rate_rad_per_frame = round(self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame,5) config.output_limit_rps = round(self.vector_data.config_param.ctlconfig.output_limit_rps,3) config.input_target_limit_rps = round(self.vector_data.config_param.ctlconfig.input_target_limit_rps,3) if (True == config.send_unlock_request): try: key = int(config.unlock_key,16) self._unlock_gain_tuning(key) except: rospy.logerr("Invalid Key Value!!!! Must be a string in 32bit hex format") config.send_unlock_request = False self.valid_config = config if (True == load_config_params) or (False == self.param_server_initialized): self.update_base_local_planner = True self._update_move_base_params() self.param_server_initialized = True self.parameter_server_is_updating = False return config def _update_move_base_params(self): """ If parameter updates have not been called in the last 5 seconds allow the subscriber callback to set them """ if ((rospy.Time.now().to_sec()-self.last_move_base_update) > 5.0): self.update_base_local_planner = True if self.update_base_local_planner: self.update_base_local_planner = False self.last_move_base_update = rospy.Time.now().to_sec() try: dyn_reconfigure_client= Client("/move_base/DWAPlannerROS",timeout=1.0) changes = dict() changes['acc_lim_x'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_y'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2) changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2 changes['max_vel_x'] = self.valid_config.x_vel_limit_mps changes['max_vel_y'] = self.valid_config.y_vel_limit_mps changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps dyn_reconfigure_client.update_configuration(changes) dyn_reconfigure_client.close() except: pass rospy.loginfo("Vector Driver updated move_base parameters to match machine parameters") def _unlock_gain_tuning(self,key): r = rospy.Rate(10) attempts = 0 while (False == self.vector_data.config_param.ctlconfig.control_tuning_unlocked) and (attempts < 3): cmds = [GENERAL_PURPOSE_CMD_ID,[2001,key]] self._add_command_to_queue(cmds) attempts += 1 r.sleep() if (True == self.vector_data.config_param.ctlconfig.control_tuning_unlocked): rospy.loginfo("Controller tuning successfully unlocked") else: rospy.logerr("Failed to unlock controller tuning, the key is likely incorrect") def _continuous_data(self,start_cont): set_continuous = [GENERAL_PURPOSE_CMD_ID,[GENERAL_PURPOSE_CMD_SEND_CONTINUOUS_DATA,start_cont]] ret = False if (True == start_cont): r = rospy.Rate(10) start_time = rospy.Time.now().to_sec() while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (True == self.vector_data.status.init): self._add_command_to_queue(set_continuous) r.sleep() ret = not self.vector_data.status.init else: r = rospy.Rate(5) start_time = rospy.Time.now().to_sec() while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == ret): self._add_command_to_queue(set_continuous) rospy.sleep(0.6) if ((rospy.Time.now().to_sec() - self.last_rsp_rcvd) > 0.5): ret = True r.sleep() self.vector_data.status.init = True return ret def _extract_faultlog(self): r = rospy.Rate(2) start_time = rospy.Time.now().to_sec() while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (True == self.extracting_faultlog): self._add_command_to_queue([GENERAL_PURPOSE_CMD_ID,[GENERAL_PURPOSE_CMD_SEND_FAULTLOG,0]]) r.sleep() return not self.extracting_faultlog def _force_machine_configuration(self,echo=False): """ Load all the parameters on the machine at startup; first check if they match, if they do continue. Otherwise load them and check again. """ r = rospy.Rate(5) start_time = rospy.get_time() params_loaded = False new_params = False while ((rospy.get_time() - start_time) < 10.0) and (False == params_loaded): load_params = False for i in range(NUMBER_OF_CONFIG_PARAM_VARIABLES): if (self.vector_data.config_param.configuration_feedback[i] != self.valid_config_cmd[1][i]): load_params = True if (True == load_params): self._add_command_to_queue(self.valid_config_cmd) new_params = True r.sleep() else: params_loaded = True if (new_params == True) or (echo == True): rospy.loginfo("New Machine Parameters Loaded.......") rospy.loginfo("x_vel_limit_mps: %.4f"%self.vector_data.config_param.machcfg.x_vel_limit_mps) rospy.loginfo("y_vel_limit_mps: %.4f"%self.vector_data.config_param.machcfg.y_vel_limit_mps) rospy.loginfo("accel_limit_mps2: %.4f"%self.vector_data.config_param.machcfg.accel_limit_mps2) rospy.loginfo("decel_limit_mps2: %.4f"%self.vector_data.config_param.machcfg.decel_limit_mps2) rospy.loginfo("dtz_decel_limit_mps2: %.4f"%self.vector_data.config_param.machcfg.dtz_decel_limit_mps2) rospy.loginfo("yaw_rate_limit_rps: %.4f"%self.vector_data.config_param.machcfg.yaw_rate_limit_rps) rospy.loginfo("yaw_accel_limit_rps2: %.4f"%self.vector_data.config_param.machcfg.yaw_accel_limit_rps2) rospy.loginfo("wheel_diameter_m: %.4f"%self.vector_data.config_param.machcfg.wheel_diameter_m) rospy.loginfo("wheel_base_length_m: %.4f"%self.vector_data.config_param.machcfg.wheelbase_length_m) rospy.loginfo("wheel_track_width_m: %.4f"%self.vector_data.config_param.machcfg.wheel_track_width_m) rospy.loginfo("gear_ratio: %.4f"%self.vector_data.config_param.machcfg.gear_ratio) rospy.loginfo("motion_while_charging: %u"%(((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1)) rospy.loginfo("motion_ctl_input_filter: %u\n"%((self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK)) elif not params_loaded: rospy.logerr("Failed to load machine parameters!!!!!!!!!!") return params_loaded def _force_pid_configuration(self,echo=False): r = rospy.Rate(5) start_time = rospy.get_time() params_loaded = False new_params = False while ((rospy.get_time() - start_time) < 10.0) and (False == params_loaded): load_params = False for i in range(16,(16+NUMBER_OF_PID_PARAM_VARIABLES)): if (self.vector_data.config_param.configuration_feedback[i] != self.valid_pid_cmd[1][i-16]): load_params = True if (True == load_params): self._add_command_to_queue(self.valid_pid_cmd) r.sleep() new_params = True else: params_loaded = True if (new_params == True) or (echo == True): rospy.loginfo("New PID Parameters Loaded.......") rospy.loginfo("p_gain_rps_per_rps: %.4f"%self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps) rospy.loginfo("i_gain_rps_per_rps: %.4f"%self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad) rospy.loginfo("d_gain_rps_per_rps: %.4f"%self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2) rospy.loginfo("fdfwd_gain_rps_per_motor_rps: %.4f"%self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps) rospy.loginfo("p_error_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.p_error_limit_rps) rospy.loginfo("i_error_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.i_error_limit_rad) rospy.loginfo("d_error_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.d_error_limit_rps2) rospy.loginfo("i_error_drain_rate_rad_per_frame: %.4f"%self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame) rospy.loginfo("output_limit_rps: %.4f"%self.vector_data.config_param.ctlconfig.output_limit_rps) rospy.loginfo("input_target_limit_rps: %.4f\n"%self.vector_data.config_param.ctlconfig.input_target_limit_rps) elif not params_loaded: rospy.logerr("Failed to load machine parameters!!!!!!!!!!") return params_loaded
#!/usr/bin/env python3 import rospy from param_examples.cfg import dynamic_paramConfig from dynamic_reconfigure.server import Server def callback(config, level): rospy.loginfo( "Get these parameters int_param:{int_param}, str_param:{str_param}, bool_param:{bool_param}" .format(**config)) return config if __name__ == '__main__': rospy.init_node('dynamic_param_examples') try: Server = Server(dynamic_paramConfig, callback) rospy.spin() except rospy.ROSException as e: print(e)
self.client.set_user() marker = '%s:%s' % (config.type_of_marker, config.marker) self.client.set_marker(marker) self.mute = config.mute self.insert_behavior = config.insert_behavior if config.preset_user and config.preset_user != self.current_user: self.current_user = config.preset_user config.user = '' logger.info("Set preset user %s" % self.current_user) if config.user and config.user != self.current_user: self.current_user = config.user config.preset_user = '' logger.info("Set current user %s" % self.current_user) if config.reset_session: self.client.reset_session() config.reset_session = Fales return config if __name__ == '__main__': rospy.init_node('chatbot') bot = Chatbot() from rospkg import RosPack rp = RosPack() data_dir = os.path.join(rp.get_path('chatbot'), 'scripts/aiml') sent3_file = os.path.join(data_dir, "senticnet3.props.csv") bot.polarity.load_sentiment_csv(sent3_file) Server(ChatbotConfig, bot.reconfig) rospy.spin()
def __init__(self, options): self.options = options self.crazyflie = Crazyflie() self.battery_monitor = BatteryMonitor() cflib.crtp.init_drivers() self.zspeed = zSpeed() # Keep track of this to know if the user changed imu mode to/from imu6/9 self.preMagcalib = None #self.csvwriter = csv.writer(open('baro.csv', 'wb'), delimiter=',',quotechar='"', quoting=csv.QUOTE_MINIMAL) #self.baro_start_time = None # Some shortcuts self.HZ100 = 10 self.HZ10 = 100 self.HZ1 = 1000 self.HZ500 = 2 self.HZ250 = 4 self.HZ125 = 8 self.HZ50 = 20 # Callbacks the CF driver calls self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinishedCB) self.crazyflie.connectionFailed.add_callback(self.connectionFailedCB) self.crazyflie.connectionInitiated.add_callback(self.connectionInitiatedCB) self.crazyflie.disconnected.add_callback(self.disconnectedCB) self.crazyflie.connectionLost.add_callback(self.connectionLostCB) self.crazyflie.linkQuality.add_callback(self.linkQualityCB) # Advertise publishers self.pub_acc = rospy.Publisher("/cf/acc", accMSG) self.pub_mag = rospy.Publisher("/cf/mag", magMSG) self.pub_gyro = rospy.Publisher("/cf/gyro", gyroMSG) self.pub_baro = rospy.Publisher("/cf/baro", baroMSG) self.pub_motor = rospy.Publisher("/cf/motor", motorMSG) self.pub_hover = rospy.Publisher("/cf/hover", hoverMSG) self.pub_attitude = rospy.Publisher("/cf/attitude", attitudeMSG) self.pub_bat = rospy.Publisher("/cf/bat", batMSG) self.pub_zpid = rospy.Publisher("/cf/zpid", zpidMSG) self.pub_tf = tf.TransformBroadcaster() # Subscribers self.sub_tf = tf.TransformListener() self.sub_joy = rospy.Subscriber("/cfjoy", joyMSG, self.new_joydata) self.sub_magCalib = rospy.Subscriber("/magCalib", magCalibMSG, self.new_magCalib) # Keep track of link quality to send out with battery status self.link = 0 # Loggers self.logMotor = None self.logBaro = None self.logMag = None self.logGyro = None self.logAttitude = None self.logAcc = None self.logBat = None self.logHover = None self.logZPid = None self.logGravOffset = None #only here till it works # keep tracks if loggers are running self.acc_monitor = False self.gyro_monitor = False self.baro_monitor = False self.mag_monitor = False self.bat_monitor = False self.motor_monitor = False self.attitude_monitor = False self.hover_monitor = False self.zpid_monitor = False # CF params we wanna be able to change self.cf_param_groups = ["hover", "sensorfusion6", "magCalib"] self.cf_params_cache = {} # Dynserver self.dynserver = DynamicReconfigureServer(driverCFG, self.reconfigure) # Finished set up. Connect to flie self.connect(self.options)
def __init__(self): self.simulate = rospy.get_param("simulate") self.use_ground_truth = rospy.get_param("use_ground_truth") self.e1 = 0.0 self.e2 = 0.0 self.data_lock = threading.RLock() # 0 =integral-SMC, 1=PID-Controller, 2=SMC with separate i self.controller_type = 0 # PD-Controller, k_d / k_p ~= 0.6 self.k_p = 9.0 self.k_d = 5.8 # SMC self.alpha = 2.5 self.Lambda = 1.0 self.kappa = 1.2 self.epsilon = 0.7 self.desired_y_pos = -0.5 self.desired_y_velocity = 0.0 self.desired_y_acceleration = 0.0 self.current_y_pos = None self.current_y_velocity = None self.y_uncertainty = 1.0 self.k_u = 2.0 self.pose_msg_time = 0.0 self.twist_msg_time = 0.0 self.max_msg_timeout = 0.1 self.min_y_limit = 0.1 self.max_y_limit = 3.3 self.y_d_limit = 0.7 rospy.init_node("yController") self.lateral_thrust_pub = rospy.Publisher("lateral_thrust", Float64, queue_size=1) self.error_pub = rospy.Publisher("y_control_error", StateVector2D, queue_size=1) self.controller_ready_pub = rospy.Publisher("y_controller_ready", Bool, queue_size=1) self.pose_sub = rospy.Subscriber("ekf_pose", PoseWithCovarianceStamped, self.get_current_pose, queue_size=1) self.twist_sub = rospy.Subscriber("ekf_twist", TwistWithCovarianceStamped, self.get_current_twist, queue_size=1) rospy.sleep(5.0) self.report_readiness(True) self.server = Server(YControlConfig, self.server_callback) self.setpoint_sub = rospy.Subscriber("y_setpoint", StateVector3D, self.get_setpoint, queue_size=1)
class img_interface_node: ## This is the LISTENER for the layer ABOVE """The 'img_interface_node' is the class which communicates to the layer above according to this structure. It provides services and dynamic reconfigure server and also publishes topics.""" ############################################### ## ***** Static Data Members ***** ############################################### # **** Services provided to the above layer #================================================== listOf_services = {} ############################################### ## ***** Constructor Method ***** ############################################### 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 ################################################### ## ***** General Purpose Methods ***** ################################################### # **** Launch services for receiving requests #================================================== def listenToRequests(self): """Main services creator method in order to remain listening for requests. It receives nothing and returns true just for future needs.""" self.listOf_services['get/StringProperty'] = rospy.Service('get/StringProperty', stringValue, self.getStringProperty) self.listOf_services['get/IntProperty'] = rospy.Service('get/IntProperty', intValue, self.getIntProperty) self.listOf_services['get/FloatProperty'] = rospy.Service('get/FloatProperty', floatValue, self.getFloatProperty) self.listOf_services['get/BoolProperty'] = rospy.Service('get/BoolProperty', booleanValue, self.getBoolProperty) ##MICHI: 14Feb2012 self.listOf_services['get/DispImage'] = rospy.Service('get/DispImage', disparityImage, self.getDispImage) self.listOf_services['get/Image'] = rospy.Service('get/Image', normalImage, self.getImage) ##MICHI: 13Mar2012 self.listOf_services['publishDispImages'] = rospy.Service('publishDispImages', requestTopic, self.publishDispImages) self.listOf_services['publishImages'] = rospy.Service('publishImages', requestTopic, self.publishImages) ##MICHI: 16Apr2012 self.listOf_services['get/TopicLocation'] = rospy.Service('get/TopicLocation', stringValue, self.getStringProperty) self.listOf_services['set/TopicLocation'] = rospy.Service('set/TopicLocation', setString, self.setTopicLocation) self.listOf_services['set/StrProperty'] = rospy.Service('set/StrProperty', setString, self.setStrProperty) self.listOf_services['set/IntProperty'] = rospy.Service('set/IntProperty', setInteger, self.setIntProperty) self.listOf_services['set/FloatProperty'] = rospy.Service('set/FloatProperty', setFloat, self.setFloatProperty) self.listOf_services['set/BoolProperty'] = rospy.Service('set/BoolProperty', setBoolean, self.setBoolProperty) rospy.loginfo("Ready to answer service requests.") return True ################################################### # **** Dynamic Reconfigure related methods #================================================== def dynServerCallback(self, dynConfiguration, levelCode): """Handler for the changes in the Dynamic Reconfigure server Must return a configuration in the same format as received.""" if self.avoidRemoteReconf > 0: rospy.loginfo("LOCAL config was changed by self (levelCode = %s)"%levelCode) self.avoidRemoteReconf = self.avoidRemoteReconf - 1 elif levelCode != 0: rospy.loginfo("LOCAL configuration changed.".rjust(80, '_')) for elem in dynConfiguration: if self.translator.canSet(elem): success = self.setAnyProperty(elem, dynConfiguration[elem]) if success == False or success == None: rospy.logerr('|__> ...error... while setting property "%s"'%elem) rospy.logerr("Changing %s failed with %s..."%(elem,dynConfiguration[elem])) dynConfiguration[elem] = self.getAnyProperty(elem) rospy.logdebug("...%s used"%(dynConfiguration[elem])) return dynConfiguration def updateSelfParameters(self, dynConfiguration, avoidPropagation = True): """This method is responsible for changing the node's own dynamic reconfigure parameters from its own code ***but avoiding a chain of uncontrolled callbacks!!. It returns True if everything went as expected.""" newConfig = {} for elem in dynConfiguration: paramName = self.translator.reverseInterpret(elem) if paramName != "": newValue = dynConfiguration[elem] newConfig[paramName] = newValue else: rospy.logerr("Error while retrieving reverse translation.") if avoidPropagation == True: self.avoidRemoteReconf = self.avoidRemoteReconf + 1 requestResult = self.dynServer.update_configuration(newConfig) if requestResult != None: return requestResult return False def setTopicLocation(self, setStrMsg): rospy.loginfo (str("Received call to set/TopicLocation " + setStrMsg.topicName + " " + setStrMsg.newValue)) translation = self.translator.interpret(setStrMsg.topicName) if translation != None and (translation[PPTY_KIND]=="publishedTopic" or translation[PPTY_KIND]=="topic"): oldAddress = translation[PPTY_REF] elif manager3D.is_published(setStrMsg.topicName): oldAddress = setStrMsg.topicName else: print str("The " + setStrMsg.topicName + " topic wasn't found neither as topic or as a name:") return str("Exception while relocating. Topic not found.") try: rospy.loginfo (str("...relocating " + oldAddress + "...")) self.driverMgr.relocateTopic(oldAddress, setStrMsg.newValue) ## If it was a name; the value needs to be updated in the translator if oldAddress == setStrMsg.topicName: self.translator.updateValue(setStrMsg.topicName, setStrMsg.newValue) except Exception as e: rospy.logerr("Exception while relocating: %s"%(e)) return str("Exception while relocating: %s"%(e)) return "Success!" ################################################### # **** Generic handlers for getting and setting parameters #================================================== def setAnyProperty(self, propertyName, newValue): """Generic setter in order to redirect to the type-specific setter method.""" propertyData = self.translator.interpret(propertyName) if propertyData == None: return None #else: if (propertyData[PPTY_TYPE].lower()).find("string") >= 0: # if type(newValue) != str: # print "Error: value '%s' seems to be %s instead of '%s' which is needed"%(newValue, type(newValue), propertyData[PPTY_TYPE]) # return None valueType = str elif (propertyData[PPTY_TYPE].lower()).find("int") >= 0: valueType = int elif (propertyData[PPTY_TYPE].lower()).find("double") >= 0 or (propertyData[PPTY_TYPE].lower()).find("float") >= 0: valueType = float elif (propertyData[PPTY_TYPE].lower()).find("bool") >= 0: valueType = bool else: rospy.logerr("Error: non registered value type.") return None return self.setFixedTypeProperty(propertyName, newValue, valueType) #################################### # Specific fixed type property setters #=================================== ## The next methods are the callbacks for the setter services ##all of them are supposed to return the resulting values to ##inform in case the set event failed. ## In case there's an error; the error message is sent no ##matter the returning type is def setStrProperty(self, setterMessage): return self.setFixedTypeProperty(setterMessage.topicName, setterMessage.newValue, str) def setIntProperty(self, setterMessage): return self.setFixedTypeProperty(setterMessage.topicName, setterMessage.newValue, int) def setFloatProperty(self, setterMessage): return self.setFixedTypeProperty(setterMessage.topicName, setterMessage.newValue, float) def setBoolProperty(self, setterMessage): return self.setFixedTypeProperty(setterMessage.topicName, setterMessage.newValue, bool) def setFixedTypeProperty(self, propertyName, newValue, valueType): """Main property setter receiving the property name, the value to assign and its type and returning the response from the "setParameter" method from the low level driver.""" propertyData = self.translator.interpret(propertyName) #TODO: Any type checking here? if propertyData == None: rospy.logerr("Error: trying to set not found property.") return None # else: # print "Obtained %s = %s."%(propertyName, propertyData[PPTY_REF]) resp1 = True if propertyData[PPTY_KIND] == "dynParam": paramName = propertyTranslator.getBasename(propertyData[PPTY_REF]) resp1 = self.driverMgr.setParameter(paramName, newValue, self.translator.getDynServerPath(paramName), self) elif propertyData[PPTY_KIND] == "topic": self.driverMgr.sendByTopic(rospy.get_namespace() + propertyData[PPTY_REF], newValue, remoteType[valueType]) else: resp1 = False rospy.logerr("Error: unable to set property %s of kind: '%s'"%(propertyName, propertyData[PPTY_KIND])) return valueType(resp1) # Getter and getter handlers # Generic setter in order to redirect to the appropriate method def getAnyProperty(self, propertyName): """Generic getter in order to redirect to the type-specific getter method.""" propertyData = self.translator.interpret(propertyName) if propertyData == None: return None #else: if propertyData[PPTY_TYPE].find("string") >= 0: valueType = str elif (propertyData[PPTY_TYPE].lower()).find("int") >= 0: valueType = int elif (propertyData[PPTY_TYPE].lower()).find("double") >= 0 or (propertyData[PPTY_TYPE].lower()).find("float") >= 0: valueType = float elif (propertyData[PPTY_TYPE].lower()).find("bool") >= 0: valueType = bool else: rospy.logerr("Error: non registered value type") return None return self.getFixedTypeProperty(propertyName, valueType) # Generic getter in order to redirect to the appropriate method #Specific fixed type property getters def getStringProperty(self, srvMsg): return self.getFixedTypeProperty(srvMsg.topicName, str) def getIntProperty(self, srvMsg): return self.getFixedTypeProperty(srvMsg.topicName, int) def getFloatProperty(self, srvMsg): return self.getFixedTypeProperty(srvMsg.topicName, float) def getBoolProperty(self, srvMsg): return self.getFixedTypeProperty(srvMsg.topicName, bool) def getDispImage(self, srvMsg): respImages = [] while len(respImages) < srvMsg.nImages: respImages.append(self.getFixedTypeProperty(srvMsg.topicName, None)) for image in respImages: self.driverMgr.sendByTopic("testImages", image, DisparityImage) return respImages def getImage(self, srvMsg): respImages = [] while len(respImages) < srvMsg.nImages: respImages.append(self.getFixedTypeProperty(srvMsg.topicName, None)) return respImages ##AMPLIATION: I could add normalImage.timestamp (time[] timestamp) if needed (including the changes in the "normalImage.srv" ## in that case I would need a normalImage variable and set both fields: images and timestamp def getFixedTypeProperty(self, propertyName, valueType): """Main property getter receiving the property name and the value type and returning the current value from the low level driver.""" propertyData = self.translator.interpret(propertyName) if propertyData == None: return None #else: if propertyData[PPTY_KIND] == "dynParam": paramName = propertyTranslator.getBasename(propertyData[PPTY_REF]) resp1 = self.driverMgr.getParameter(paramName, self.translator.getDynServerPath(propertyName)) elif propertyData[PPTY_KIND] == "publishedTopic": resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF], valueType) elif propertyData[PPTY_KIND] == "readOnlyParam": paramName = propertyTranslator.getBasename(propertyData[PPTY_REF]) location = rospy.search_param(param_name) resp1 = rospy.get_param(location) elif propertyData[PPTY_KIND] == "topic": ##MICHI: 14Feb2012 if valueType == str: resp1 = str(propertyData[PPTY_REF]) else: ## Special case for reading disparity images valueType2 = DisparityImage if propertyData[PPTY_TYPE].find("Disparity") < 0: valueType2 = Image resp1 = self.driverMgr.getTopic(propertyData[PPTY_REF], valueType2) else: rospy.logerr("Error: unable to get property %s of kind %s"%(propertyName, propertyData[PPTY_TYPE])) resp1 = None return resp1 # Method to request the complete list of properties def get_property_list(self): """Auxiliary method for receiving the list of properties known by the translator.""" return self.translator.get_property_list() # Methods related to requesting topics to publish def publishDispImages(self, srvMsg): """Specific purpose method meant to retransmit a disparity image topic on a specific topic path receiving the original path, the new path and the amount of messages. It returns a message telling if it worked.""" topicPath = self.translator.get_topic_path(srvMsg.sourceTopic) self.driverMgr.retransmitTopic(srvMsg.nImages, topicPath, srvMsg.responseTopic, DisparityImage) "%s images sent from %s topic to %s."%(srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic) return "%s images sent from %s topic to %s."%(srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic) def publishImages(self, srvMsg): """Specific purpose method meant to retransmit an image topic on a specific topic path receiving the original path, the new path and the amount of messages. It returns a message telling if it worked.""" topicPath = self.translator.get_topic_path(srvMsg.sourceTopic) self.driverMgr.retransmitTopic(srvMsg.nImages, topicPath, srvMsg.responseTopic, Image) "%s images sent from %s topic to %s."%(srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic) return "%s images sent from %s topic to %s."%(srvMsg.nImages, srvMsg.sourceTopic, srvMsg.responseTopic)
#Map message cost_map = OccupancyGrid() #Map suscriber rospy.Subscriber("move_base/global_costmap/costmap", OccupancyGrid, callback) #Controller init kobuki_controller = controller() #Dynamic reconfigure flag dyn_flag = 0 #Dynamic reconfigure server initialization srv = Server(controllerConfig, srv_callback) #Wheel speed publisher nameSpeedTopic = "/mobile_base/commands/velocity" kobuki_speed_pub = rospy.Publisher(nameSpeedTopic, Twist, queue_size=10) command = Twist() #Make a pause before recieving map print("Retrieving Map") time.sleep(1) print("Map Data: ") print(cost_map.data) print("Map Info: ") print(cost_map.info) #Map dimmensions
return config if __name__ == "__main__": rospy.init_node("example", anonymous=True) mach_pub = rospy.Publisher('mach', Mach_msg, queue_size=5) spare_function_pub = rospy.Publisher('spare_function_out', spare_function_out, queue_size=5) spare_function_para_pub = rospy.Publisher('spare_function_para', spare_function_para, queue_size=5) rospy.Subscriber("sensor_kalman_msg", Sensor_msg, sensorCallback) config_srv = Server(spare_function_Config, getConfigCallback) rate = rospy.Rate(10) goal_list = [(20, 0), (-55, 70), (10, -10)] k = 0 goal_fix = 0 try: while not rospy.is_shutdown(): sa, ra = module(sensor_submsg, goal_list[k]) distance = math.sqrt((goal_list[k][0] - sensor_submsg[4]) * (goal_list[k][0] - sensor_submsg[4]) + (goal_list[k][1] - sensor_submsg[5]) * (goal_list[k][1] - sensor_submsg[5])) if distance < 3: k = k + 1 if k > len(goal_list) - 1:
#!/usr/bin/env python import rospy from dynamic_reconfigure.server import Server from quanergy_client_ros.cfg import resolutionConfig def callback(config, level): rospy.loginfo("""Reconfigure Request: {int_param}""".format(**config)) return config if __name__ == "__main__": rospy.init_node("quanergy_client_ros_tut", anonymous = False) srv = Server(resolutionConfig, callback) rospy.spin()
response.success = self._planner.add_planning_scene_object(object_name=request.object_identifier, object_class_name=request.class_identifier, pose=transform_matrix) return response def handle_remove_object_request(self, request): response = RemoveObjectResponse() response.success = self._planner.remove_planning_scene_object(object_name=request.object_identifier) return response if __name__ == "__main__": rospy.init_node('hfts_integrated_planner_node', log_level=rospy.DEBUG) # reconnect logging calls to ros log system logging.getLogger().addHandler(rosgraph.roslogging.RosStreamHandler()) # logs sent to children of trigger with a level >= this will be redirected to ROS logging.getLogger().setLevel(logging.DEBUG) # Build handler handler = HandlerClass() srv = Server(IntegratedHFTSPlannerConfig, handler.update_parameters) # Create services add_obj_service = rospy.Service(rospy.get_name() + '/add_object', AddObject, handler.handle_add_object_request) remove_obj_service = rospy.Service(rospy.get_name() + '/remove_object', RemoveObject, handler.handle_remove_object_request) planning_service = rospy.Service(rospy.get_name() + '/plan_fingertip_grasp_motion', PlanGraspMotion, handler.handle_plan_request) arm_planning_service = rospy.Service(rospy.get_name() + '/plan_arm_motion', PlanArmMotion, handler.handle_move_arm_request) # Spin until node is killed rospy.spin() sys.exit(0)
def __init__(self): # Give the node a name rospy.init_node('calibrate_angular', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = rospy.get_param('~rate', 20) r = rospy.Rate(self.rate) # The test angle is 360 degrees self.test_angle = radians(rospy.get_param('~test_angle', 360.0)) self.speed = rospy.get_param('~speed', 1.0) # radians per second self.tolerance = radians(rospy.get_param('tolerance', 1)) # degrees converted to radians self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0) self.start_test = rospy.get_param('~start_test', True) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # Fire up the dynamic_reconfigure server dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback) # Connect to the dynamic_reconfigure server dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60) # The base frame is usually base_link or base_footprint self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") reverse = 1 while not rospy.is_shutdown(): if self.start_test: # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() last_angle = self.odom_angle turn_angle = 0 self.test_angle *= reverse error = self.test_angle - turn_angle # Alternate directions between tests reverse = -reverse while abs(error) > self.tolerance and self.start_test: if rospy.is_shutdown(): return # Rotate the robot to reduce the error move_cmd = Twist() move_cmd.angular.z = copysign(self.speed, error) self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() # Compute how far we have gone since the last measurement delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle) # Add to our total angle so far turn_angle += delta_angle # Compute the new error error = self.test_angle - turn_angle # Store the current angle for the next comparison last_angle = self.odom_angle # Stop the robot self.cmd_vel.publish(Twist()) # Update the status flag self.start_test = False params = {'start_test': False} dyn_client.update_configuration(params) rospy.sleep(0.5) # Stop the robot self.cmd_vel.publish(Twist())
def __init__(self): rospy.on_shutdown(self.on_shutdown) self.update_rate = rospy.get_param("~update_rate", 10.0) self.sensor_frame_id = rospy.get_param("~sensor_frame_id", "respeaker_base") self.doa_xy_offset = rospy.get_param("~doa_xy_offset", 0.0) self.doa_yaw_offset = rospy.get_param("~doa_yaw_offset", 90.0) self.speech_prefetch = rospy.get_param("~speech_prefetch", 0.5) self.speech_continuation = rospy.get_param("~speech_continuation", 0.5) self.speech_max_duration = rospy.get_param("~speech_max_duration", 12.0) self.speech_min_duration = rospy.get_param("~speech_min_duration", 0.3) suppress_pyaudio_error = rospy.get_param("~suppress_pyaudio_error", True) self.asr_engine = rospy.get_param("~asr_engine", "google_legacy_single_utterance") rospy.loginfo("ASR Engine is: %s" % self.asr_engine) # self.respeaker = RespeakerInterface() self.prev_doa = None self.prev_is_voice = None if self.asr_engine != "silent": self.speech_audio_buffer = str() self.is_speaking = False self.speech_stopped = rospy.Time(0) self.pub_audio = rospy.Publisher("audio", AudioData, queue_size=10) self.pub_speech_audio = rospy.Publisher("speech_audio", AudioData, queue_size=10) # advertise self.pub_vad = rospy.Publisher("is_speaking", Bool, queue_size=1, latch=True) self.pub_doa_raw = rospy.Publisher("sound_direction", Int32, queue_size=1, latch=True) self.pub_doa = rospy.Publisher("sound_localization", PoseStamped, queue_size=1, latch=True) # init config self.config = None self.dyn_srv = Server(RespeakerConfig, self.on_config) # start if self.asr_engine != "silent": self.respeaker_audio = RespeakerAudio( self.on_audio, suppress_error=suppress_pyaudio_error) self.speech_prefetch_bytes = int( self.speech_prefetch * self.respeaker_audio.rate * self.respeaker_audio.bitdepth / 8.0) self.speech_prefetch_buffer = str() self.respeaker_audio.start() self.info_timer = rospy.Timer(rospy.Duration(1.0 / self.update_rate), self.on_timer) self.timer_led = None self.sub_led = rospy.Subscriber("status_led", ColorRGBA, self.on_status_led) self.pepper_last_started_speaking_timestamp = rospy.Time.now() self.pepper_is_speaking = False self.sub_pepper_speech_status = rospy.Subscriber( "pepper_speech_status", String, self.on_pepper_speech_status_change_cb)
def __init__(self): ''' Initialization of the class. ''' self.start_flag = False # flag indicates if the first measurement is received self.config_start = False # flag indicates if the config callback is called for the first time self.euler_mv = Vector3() # measured euler angles self.euler_sp = Vector3(0, 0, 0) # euler angles referent values self.euler_sp_old = Vector3(0, 0, 0) self.euler_sp_filt = Vector3(0, 0, 0) self.w_sp = 0 # referent value for motor velocity - it should be the output of height controller self.euler_rate_mv = Vector3() # measured angular velocities self.euler_rate_mv_old = Vector3() self.clock = Clock() self.pid_roll = PID() # roll controller self.pid_roll_rate = PID() # roll rate (wx) controller self.pid_pitch = PID() # pitch controller self.pid_pitch_rate = PID() # pitch rate (wy) controller self.pid_yaw = PID() # yaw controller self.pid_yaw_rate = PID() # yaw rate (wz) controller # Adding VPC controllers for roll and pitch self.pid_vpc_roll = PID() self.pid_vpc_pitch = PID() ################################################################## ################################################################## # Add your PID params here self.pid_roll.set_kp(7.0) self.pid_roll.set_ki(0.5) self.pid_roll.set_kd(0.0) self.pid_roll_rate.set_kp(190.0) self.pid_roll_rate.set_ki(15.0) self.pid_roll_rate.set_kd(3.0) self.pid_roll_rate.set_lim_high(1450.0) self.pid_roll_rate.set_lim_low(-1450.0) self.pid_pitch.set_kp(7.0) self.pid_pitch.set_ki(0.5) self.pid_pitch.set_kd(0.0) self.pid_pitch_rate.set_kp(190.0) self.pid_pitch_rate.set_ki(15.0) self.pid_pitch_rate.set_kd(3.0) self.pid_pitch_rate.set_lim_high(1450.0) self.pid_pitch_rate.set_lim_low(-1450.0) self.pid_yaw.set_kp(5.0) self.pid_yaw.set_ki(0.0) self.pid_yaw.set_kd(0.0) self.pid_yaw_rate.set_kp(180.0) self.pid_yaw_rate.set_ki(0.0) self.pid_yaw_rate.set_kd(0.0) self.pid_yaw_rate.set_lim_high(1450.0) self.pid_yaw_rate.set_lim_low(-1450.0) # VPC pids self.pid_vpc_roll.set_kp(0) self.pid_vpc_roll.set_ki(0.0) self.pid_vpc_roll.set_kd(0) self.pid_vpc_roll.set_lim_high(0.04) self.pid_vpc_roll.set_lim_low(-0.04) self.pid_vpc_pitch.set_kp(0) self.pid_vpc_pitch.set_ki(0.0) self.pid_vpc_pitch.set_kd(0) self.pid_vpc_pitch.set_lim_high(0.04) self.pid_vpc_pitch.set_lim_low(-0.04) # Filter parameters self.rate_mv_filt_K = 1.0 self.rate_mv_filt_T = 0.02 # Reference prefilters self.roll_reference_prefilter_K = 1.0 self.roll_reference_prefilter_T = 0.0 self.pitch_reference_prefilter_K = 1.0 self.pitch_reference_prefilter_T = 0.0 # Offsets for pid outputs self.roll_rate_output_trim = 0.0 self.pitch_rate_output_trim = 0.0 ################################################################## ################################################################## self.rate = rospy.get_param('~rate', 100) self.ros_rate = rospy.Rate(self.rate) # attitude control at 100 Hz self.Ts = 1.0/float(self.rate) self.t_old = 0 rospy.Subscriber('imu', Imu, self.ahrs_cb) rospy.Subscriber('mot_vel_ref', Float64, self.mot_vel_ref_cb) rospy.Subscriber('euler_ref', Vector3, self.euler_ref_cb) rospy.Subscriber('/clock', Clock, self.clock_cb) rospy.Subscriber('reset_controllers', Empty, self.reset_controllers_cb) self.attitude_pub = rospy.Publisher('attitude_command', Float64MultiArray, queue_size=1) self.pub_pid_roll = rospy.Publisher('pid_roll', PIDController, queue_size=1) self.pub_pid_roll_rate = rospy.Publisher('pid_roll_rate', PIDController, queue_size=1) self.pub_pid_pitch = rospy.Publisher('pid_pitch', PIDController, queue_size=1) self.pub_pid_pitch_rate = rospy.Publisher('pid_pitch_rate', PIDController, queue_size=1) self.pub_pid_yaw = rospy.Publisher('pid_yaw', PIDController, queue_size=1) self.pub_pid_yaw_rate = rospy.Publisher('pid_yaw_rate', PIDController, queue_size=1) self.pub_pid_vpc_roll = rospy.Publisher('pid_vpc_roll', PIDController, queue_size=1) self.pub_pid_vpc_pitch = rospy.Publisher('pid_vpc_pitch', PIDController, queue_size=1) self.cfg_server = Server(VpcMmuavAttitudeCtlParamsConfig, self.cfg_callback)
def __init__(self): # Give the node a name rospy.init_node('calibrate_linear', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = rospy.get_param('~rate', 20) r = rospy.Rate(self.rate) # Set the distance to travel self.test_distance = rospy.get_param('~test_distance', 1.0) # meters self.speed = rospy.get_param('~speed', 0.15) # meters per second self.tolerance = rospy.get_param('~tolerance', 0.01) # meters self.odom_linear_scale_correction = rospy.get_param( '~odom_linear_scale_correction', 1.0) self.start_test = rospy.get_param('~start_test', True) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # Fire up the dynamic_reconfigure server dyn_server = Server(CalibrateLinearConfig, self.dynamic_reconfigure_callback) # Connect to the dynamic_reconfigure server dyn_client = dynamic_reconfigure.client.Client("calibrate_linear", timeout=60) # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") self.position = Point() # Get the starting position from the tf transform between the odom and base frames self.position = self.get_position() x_start = self.position.x y_start = self.position.y move_cmd = Twist() while not rospy.is_shutdown(): # Stop the robot by default move_cmd = Twist() if self.start_test: # Get the current position from the tf transform between the odom and base frames self.position = self.get_position() # Compute the Euclidean distance from the target point distance = sqrt( pow((self.position.x - x_start), 2) + pow((self.position.y - y_start), 2)) # Correct the estimated distance by the correction factor distance *= self.odom_linear_scale_correction # How close are we? error = distance - self.test_distance rospy.loginfo("Error: " + str(error)) # Are we close enough? if not self.start_test or abs(error) < self.tolerance: self.start_test = False params = {'start_test': False} rospy.loginfo(params) dyn_client.update_configuration(params) else: # If not, move in the appropriate direction move_cmd.linear.x = copysign(self.speed, -1 * error) else: self.position = self.get_position() x_start = self.position.x y_start = self.position.y self.cmd_vel.publish(move_cmd) r.sleep() # Stop the robot self.cmd_vel.publish(Twist())
def main(): global ser rospy.init_node('beacon_publisher_node') pub = rospy.Publisher('beacon', ObservationRangeBeacon, queue_size=10) pub_raw = rospy.Publisher('beacon_raw', Beacon, queue_size=10) antenna_delay = rospy.get_param('~antenna_delay', 16450) sensor_frame_id = rospy.get_param('~sensor_frame_id', 'uwb_reciever_link') serial_port = rospy.get_param('~serial_port', '/dev/CP2104_Friend') srv = Server(BeaconPublisherConfig, config_callback) ser = serial.Serial(serial_port, 115200, timeout=2) # Set inital antenna delay while True: line = ser.readline() line = line.decode("utf-8") #print line if line.startswith("### TAG ###"): ser.write(str(antenna_delay).encode()) rospy.loginfo("Antenna delay is transfered.") break orb = ObservationRangeBeacon() orb.header.frame_id = sensor_frame_id orb.sensor_pose_on_robot.position.x = 0.04 orb.sensor_pose_on_robot.position.y = 0 orb.sensor_pose_on_robot.position.z = 0.26 orb.sensor_pose_on_robot.orientation.x = 0 orb.sensor_pose_on_robot.orientation.y = 0 orb.sensor_pose_on_robot.orientation.z = 0 orb.sensor_pose_on_robot.orientation.w = 1 orb.min_sensor_distance = 0.0 orb.max_sensor_distance = 120.0 orb.sensor_std_range = 0.02 orb.sensed_data.append(SingleRangeBeaconObservation()) beacon = Beacon() beacon.header.frame_id = sensor_frame_id rospy.loginfo("Starting main loop...") while not rospy.is_shutdown(): line = ser.readline() try: #print(line) obj = json.loads(line) except ValueError: continue orb.header.stamp = rospy.Time.now() orb.sensed_data[0].range = obj['r'] orb.sensed_data[0].id = obj['aa'] pub.publish(orb) beacon.header.stamp = orb.header.stamp beacon.type = obj['type'] beacon.tag_address = obj['ta'] beacon.anchor_address = obj['aa'] beacon.range = obj['r'] beacon.receive_power = obj['rxp'] beacon.first_path_power = obj['fpp'] beacon.receive_quality = obj['q'] beacon.voltage = obj['v'] beacon.temperature = obj['t'] beacon.tag_antenna_delay = obj['ad'] pub_raw.publish(beacon)
def start_spinning(self): # Create the dyanmic reconfigure server dyn_cfg_server = Server(WorkshopConfig, self.dyn_cfg_callback) rospy.spin()
#!/usr/bin/env python import rospy from dynamic_reconfigure.server import Server from autonomous_vision.cfg import CfgrobotConfig def callback(config, level): rospy.loginfo( """Reconfigure Request: {int_sensibility}, {double_lspeed},{double_param}, {bool_param}""" .format(**config)) print(config["int_sensibility"]) return config if __name__ == "__main__": rospy.init_node("autonomous_vision_cfg", anonymous=True) srv = Server(CfgrobotConfig, callback) rospy.spin()
def listener(self): dyn_srv = Server(pidConfig, self.callback_reconf) rospy.Subscriber('/scan_registration/tarDir', Pose2D, self.callback_pose) rospy.loginfo("Controller spinning..") rospy.spin()
class OGridServer: def __init__(self, frame_id='enu', map_size=500, resolution=0.3, rate=1): self.frame_id = frame_id self.ogrids = {} self.odom = None # Some default values self.plow = True self.plow_factor = 0 self.ogrid_min_value = -1 self.draw_bounds = False self.resolution = resolution self.ogrid_timeout = 2 self.enforce_bounds = False self.enu_bounds = [[0, 0, 1], [0, 0, 1], [0, 0, 1], [0, 0, 1]] # Default to centering the ogrid position = np.array([-(map_size * resolution) / 2, -(map_size * resolution) / 2, 0]) quaternion = np.array([0, 0, 0, 1]) self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion) self.global_ogrid = self.create_grid((map_size, map_size)) def set_odom(msg): return setattr(self, 'odom', mil_tools.pose_to_numpy(msg.pose.pose)) rospy.Subscriber('/odom', Odometry, set_odom) self.publisher = rospy.Publisher('/ogrid_master', OccupancyGrid, queue_size=1) self.ogrid_server = Server(OgridConfig, self.dynamic_cb) self.dynam_client = Client("bounds_server", config_callback=self.bounds_cb) self.ogrid_server.update_configuration({'width': 500}) rospy.Service("/center_ogrid", Trigger, self.center_ogrid) rospy.Timer(rospy.Duration(1.0 / rate), self.publish) def center_ogrid(self, srv): fprint("CENTERING OGRID AT POSITION", msg_color='blue') if self.odom is None: return dim = -(self.map_size[0] * self.resolution) / 2 new_org = self.odom[0] + np.array([dim, dim, 0]) config = {} config['origin_x'] = float(new_org[0]) config['origin_y'] = float(new_org[1]) config['set_origin'] = True self.ogrid_server.update_configuration(config) def bounds_cb(self, config): fprint("BOUNDS DYNAMIC CONFIG UPDATE!", msg_color='blue') if hasattr(config, "enu_1_lat"): self.enu_bounds = [[config['enu_1_lat'], config['enu_1_long'], 1], [config['enu_2_lat'], config['enu_2_long'], 1], [config['enu_3_lat'], config['enu_3_long'], 1], [config['enu_4_lat'], config['enu_4_long'], 1], [config['enu_1_lat'], config['enu_1_long'], 1]] self.enforce_bounds = config['enforce'] def dynamic_cb(self, config, level): fprint("OGRID DYNAMIC CONFIG UPDATE!", msg_color='blue') topics = config['topics'].replace(' ', '').split(',') replace_topics = config['replace_topics'].replace(' ', '').split(',') new_grids = {} for topic in topics: new_grids[topic] = OGrid(topic) if topic not in self.ogrids else self.ogrids[topic] for topic in replace_topics: new_grids[topic] = OGrid(topic, replace=True) if topic not in self.ogrids else self.ogrids[topic] self.ogrids = new_grids map_size = map(int, (config['height'], config['width'])) self.map_size = map_size self.plow = config['plow'] self.plow_factor = config['plow_factor'] self.ogrid_min_value = config['ogrid_min_value'] self.draw_bounds = config['draw_bounds'] self.resolution = config['resolution'] self.ogrid_timeout = config['ogrid_timeout'] if config['set_origin']: fprint("Setting origin!") position = np.array([config['origin_x'], config['origin_y'], 0]) quaternion = np.array([0, 0, 0, 1]) self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion) else: position = np.array([-(map_size[1] * self.resolution) / 2, -(map_size[0] * self.resolution) / 2, 0]) quaternion = np.array([0, 0, 0, 1]) self.origin = mil_tools.numpy_quat_pair_to_pose(position, quaternion) self.global_ogrid = self.create_grid(map_size) return config def create_grid(self, map_size): """ Creates blank ogrids for everyone for the low low price of $9.95! `map_size` should be in the form of (h, w) """ ogrid = OccupancyGrid() ogrid.header.stamp = rospy.Time.now() ogrid.header.frame_id = self.frame_id ogrid.info.resolution = self.resolution ogrid.info.width = map_size[1] ogrid.info.height = map_size[0] ogrid.info.origin = self.origin # TODO: Make sure this produces the correct sized ogrid ogrid.data = np.full((map_size[1], map_size[0]), -1).flatten() # fprint('Created Occupancy Map', msg_color='blue') return ogrid def publish(self, *args): # fprint("Merging Maps", newline=2) global_ogrid = deepcopy(self.global_ogrid) np_grid = numpyify(global_ogrid) # Global ogrid (only compute once) corners = get_enu_corners(global_ogrid) index_limits = transform_enu_to_ogrid(corners, global_ogrid)[:, :2] g_x_min = index_limits[0][0] g_x_max = index_limits[1][0] g_y_min = index_limits[0][1] g_y_max = index_limits[1][1] to_add = [o for o in self.ogrids.itervalues() if not o.replace] to_replace = [o for o in self.ogrids.itervalues() if o.replace] for ogrids in [to_add, to_replace]: for ogrid in ogrids: # Hard coded 5 second timeout - probably no need to reconfig this. if ogrid.nav_ogrid is None or ogrid.callback_delta > self.ogrid_timeout: # fprint("Ogrid too old!") continue # Proactively checking for errors. # This should be temporary but probably wont be. l_h, l_w = ogrid.nav_ogrid.info.height, ogrid.nav_ogrid.info.width g_h, g_w = global_ogrid.info.height, global_ogrid.info.width if l_h > g_h or l_w > g_w: fprint("Proactively preventing errors in ogrid size.", msg_color="red") new_size = max(l_w, g_w, l_h, g_h) self.global_ogrid = self.create_grid([new_size, new_size]) # Local Ogrid (get everything in global frame though) corners = get_enu_corners(ogrid.nav_ogrid) index_limits = transform_enu_to_ogrid(corners, ogrid.nav_ogrid) index_limits = transform_between_ogrids(index_limits, ogrid.nav_ogrid, global_ogrid)[:, :2] l_x_min = index_limits[0][0] l_x_max = index_limits[1][0] l_y_min = index_limits[0][1] l_y_max = index_limits[1][1] xs = np.sort([g_x_max, g_x_min, l_x_max, l_x_min]) ys = np.sort([g_y_max, g_y_min, l_y_max, l_y_min]) # These are indicies in cell units start_x, end_x = np.round(xs[1:3]) # Grabbing indices 1 and 2 start_y, end_y = np.round(ys[1:3]) # Should be indicies l_ogrid_start = transform_between_ogrids([start_x, start_y, 1], global_ogrid, ogrid.nav_ogrid) # fprint("ROI {},{} {},{}".format(start_x, start_y, end_x, end_y)) index_width = l_ogrid_start[0] + end_x - start_x # I suspect rounding will be a source of error index_height = l_ogrid_start[1] + end_y - start_y # fprint("width: {}, height: {}".format(index_width, index_height)) # fprint("Ogrid size: {}, {}".format(ogrid.nav_ogrid.info.height, ogrid.nav_ogrid.info.width)) to_add = ogrid.np_map[l_ogrid_start[1]:index_height, l_ogrid_start[0]:index_width] # fprint("to_add shape: {}".format(to_add.shape)) # Make sure the slicing doesn't produce an error end_x = start_x + to_add.shape[1] end_y = start_y + to_add.shape[0] try: # fprint("np_grid shape: {}".format(np_grid[start_y:end_y, start_x:end_x].shape)) fprint("{}, {}".format(ogrid.topic, ogrid.replace)) if ogrid.replace: np_grid[start_y:end_y, start_x:end_x] = to_add else: np_grid[start_y:end_y, start_x:end_x] += to_add except Exception as e: fprint("Exception caught, probably a dimension mismatch:", msg_color='red') print e fprint("w: {}, h: {}".format(global_ogrid.info.width, global_ogrid.info.height), msg_color='red') if self.draw_bounds and self.enforce_bounds: ogrid_bounds = transform_enu_to_ogrid(self.enu_bounds, global_ogrid).astype(np.int32) for i, point in enumerate(ogrid_bounds[:, :2]): if i == 0: last_point = point continue cv2.line(np_grid, tuple(point), tuple(last_point), 100, 3) last_point = point if self.plow: self.plow_snow(np_grid, global_ogrid) # Clip and flatten grid np_grid = np.clip(np_grid, self.ogrid_min_value, 100) global_ogrid.data = np_grid.flatten().astype(np.int8) self.publisher.publish(global_ogrid) def plow_snow(self, np_grid, ogrid): """Remove region around the boat so we never touch an occupied cell (making lqrrt not break if something touches us). """ if self.odom is None: return np_grid p, q = self.odom yaw_rot = trns.euler_from_quaternion(q)[2] # rads boat_width = params.boat_length + params.boat_buffer + self.plow_factor # m boat_height = params.boat_width + params.boat_buffer + self.plow_factor # m x, y, _ = transform_enu_to_ogrid([p[0], p[1], 1], ogrid) theta = yaw_rot w = boat_width / ogrid.info.resolution h = boat_height / ogrid.info.resolution box = cv2.boxPoints(((x, y), (w, h), np.degrees(theta))) box = np.int0(box) cv2.drawContours(np_grid, [box], 0, 0, -1) # Draw a "boat" in the ogrid boat_width = params.boat_length + params.boat_buffer boat_height = params.boat_width + params.boat_buffer x, y, _ = transform_enu_to_ogrid([p[0], p[1], 1], ogrid) w = boat_width / ogrid.info.resolution h = boat_height / ogrid.info.resolution box = cv2.boxPoints(((x, y), (w, h), np.degrees(theta))) box = np.int0(box) cv2.drawContours(np_grid, [box], 0, 40, -1) # fprint("Plowed snow!") return np_grid
class CameraSynchronizer: 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) @staticmethod def print_threads(): threads = threading.enumerate() n = 0 for t in threads: if not t.isDaemon() and t != threading.currentThread(): try: print t.name except: print "Unknown thread ", t n = n + 1 return n def kill(self): print "\nWaiting for all threads to die..." killAsynchronousUpdaters() #time.sleep(1) while self.print_threads() > 0: print "\nStill waiting for all threads to die..." time.sleep(1) print def reconfigure(self, config, level): # print "Reconfigure", config # Reconfigure the projector. self.projector.process_update(config, level) self.prosilica_inhibit.process_update(config, level) # Reconfigure the cameras. for camera in self.cameras.values(): camera.process_update(config, level) #for trig_controller in self.trig_controllers: # trig_controller.update() #for camera in self.cameras.keys(): # camera.update() for controller in self.controllers: controller.update(); for camera in self.cameras.values(): camera.apply_update() #print config self.config = config return config def update_diagnostics(self): da = DiagnosticArray() ds = DiagnosticStatus() ds.name = rospy.get_caller_id().lstrip('/') + ": Tasks" in_progress = 0; longest_interval = 0; for updater in list(asynchronous_updaters): (name, interval) = updater.getStatus() if interval == 0: msg = "Idle" else: in_progress = in_progress + 1 msg = "Update in progress (%i s)"%interval longest_interval = max(interval, longest_interval) ds.values.append(KeyValue(name, msg)) if in_progress == 0: ds.message = "Idle" else: ds.message = "Updates in progress: %i"%in_progress if longest_interval > 10: ds.level = 1 ds.message = "Update is taking too long: %i"%in_progress ds.hardware_id = "none" da.status.append(ds) da.header.stamp = rospy.get_rostime() self.diagnostic_pub.publish(da) def spin(self): self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray) try: reset_count = 0 rospy.loginfo("Camera synchronizer is running...") controller_update_count = 0 while not rospy.is_shutdown(): if self.config['camera_reset'] == True: reset_count = reset_count + 1 if reset_count > 3: self.server.update_configuration({'camera_reset' : False}) else: reset_count = 0 self.update_diagnostics() # In case the controllers got restarted, refresh their state. controller_update_count += 1 if controller_update_count >= 10: controller_update_count = 0 for controller in self.controllers: controller.update(); rospy.sleep(1) finally: rospy.signal_shutdown("Main thread exiting") self.kill() print "Main thread exiting"
class Driver: def __init__(self, options): self.options = options self.crazyflie = Crazyflie() self.battery_monitor = BatteryMonitor() cflib.crtp.init_drivers() self.zspeed = zSpeed() # Keep track of this to know if the user changed imu mode to/from imu6/9 self.preMagcalib = None #self.csvwriter = csv.writer(open('baro.csv', 'wb'), delimiter=',',quotechar='"', quoting=csv.QUOTE_MINIMAL) #self.baro_start_time = None # Some shortcuts self.HZ100 = 10 self.HZ10 = 100 self.HZ1 = 1000 self.HZ500 = 2 self.HZ250 = 4 self.HZ125 = 8 self.HZ50 = 20 # Callbacks the CF driver calls self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinishedCB) self.crazyflie.connectionFailed.add_callback(self.connectionFailedCB) self.crazyflie.connectionInitiated.add_callback(self.connectionInitiatedCB) self.crazyflie.disconnected.add_callback(self.disconnectedCB) self.crazyflie.connectionLost.add_callback(self.connectionLostCB) self.crazyflie.linkQuality.add_callback(self.linkQualityCB) # Advertise publishers self.pub_acc = rospy.Publisher("/cf/acc", accMSG) self.pub_mag = rospy.Publisher("/cf/mag", magMSG) self.pub_gyro = rospy.Publisher("/cf/gyro", gyroMSG) self.pub_baro = rospy.Publisher("/cf/baro", baroMSG) self.pub_motor = rospy.Publisher("/cf/motor", motorMSG) self.pub_hover = rospy.Publisher("/cf/hover", hoverMSG) self.pub_attitude = rospy.Publisher("/cf/attitude", attitudeMSG) self.pub_bat = rospy.Publisher("/cf/bat", batMSG) self.pub_zpid = rospy.Publisher("/cf/zpid", zpidMSG) self.pub_tf = tf.TransformBroadcaster() # Subscribers self.sub_tf = tf.TransformListener() self.sub_joy = rospy.Subscriber("/cfjoy", joyMSG, self.new_joydata) self.sub_magCalib = rospy.Subscriber("/magCalib", magCalibMSG, self.new_magCalib) # Keep track of link quality to send out with battery status self.link = 0 # Loggers self.logMotor = None self.logBaro = None self.logMag = None self.logGyro = None self.logAttitude = None self.logAcc = None self.logBat = None self.logHover = None self.logZPid = None self.logGravOffset = None #only here till it works # keep tracks if loggers are running self.acc_monitor = False self.gyro_monitor = False self.baro_monitor = False self.mag_monitor = False self.bat_monitor = False self.motor_monitor = False self.attitude_monitor = False self.hover_monitor = False self.zpid_monitor = False # CF params we wanna be able to change self.cf_param_groups = ["hover", "sensorfusion6", "magCalib"] self.cf_params_cache = {} # Dynserver self.dynserver = DynamicReconfigureServer(driverCFG, self.reconfigure) # Finished set up. Connect to flie self.connect(self.options) def new_magCalib(self, msg): """ Receive magnetometer calibration results. Send to flie via dyn conf""" new_settings = {} #TODO new_settings["magCalib_off_x"] = msg.offset[0] new_settings["magCalib_off_y"] = msg.offset[1] new_settings["magCalib_off_z"] = msg.offset[2] new_settings["magCalib_thrust_x"] = msg.thrust_comp[0] new_settings["magCalib_thrust_y"] = msg.thrust_comp[1] new_settings["magCalib_thrust_z"] = msg.thrust_comp[2] new_settings["magCalib_scale_x"] = msg.scale[0] new_settings["magCalib_scale_y"] = msg.scale[1] new_settings["magCalib_scale_z"] = msg.scale[2] self.dynserver.update_configuration(new_settings) rospy.loginfo("Updated magnetometer calibration data") def connect(self, options): """ Look for crazyflie every 2s and connect to the specified one if found""" rospy.loginfo("Waiting for crazyflie...") while not rospy.is_shutdown(): interfaces = cflib.crtp.scan_interfaces() if len(interfaces)>1: radio = None rospy.loginfo("Found: ") for i in interfaces: rospy.loginfo("--> %s [%s]", i[0],i[1]) if i[0].startswith(options.uri): radio = i[0] if radio!=None: self.crazyflie.open_link(radio) break else: rospy.sleep(2) def new_joydata(self, joymsg): """ Joydata arrived. Should happen at 100hz.""" # Parse joydata and extract commands roll = joymsg.roll pitch = joymsg.pitch yawrate = joymsg.yaw thrust = joymsg.thrust hover = joymsg.hover set_hover = joymsg.hover_set hover_change = joymsg.hover_change # Send to flie self.send_control((roll, pitch, yawrate, thrust, hover, set_hover, hover_change)) def reconfigure(self, config, level): """ Fill in local variables with values received from dynamic reconfigure clients (typically the GUI).""" config = self.zspeed.reconfigure(config) self.deadband = config["deadband"] # On / off logging if self.zpid_monitor != config["read_zpid"]: self.zpid_monitor = config["read_zpid"] if self.logZPid != None: if self.zpid_monitor: self.logZPid.start() rospy.loginfo("zPID Logging started") else: self.logZPid.stop() rospy.loginfo("zPID Logging stopped") if self.gyro_monitor != config["read_gyro"]: self.gyro_monitor = config["read_gyro"] if self.logGyro != None: if self.gyro_monitor: self.logGyro.start() rospy.loginfo("Gyro Logging started") else: self.logGyro.stop() rospy.loginfo("Gyro Logging stopped") if self.hover_monitor != config["read_hover"]: self.hover_monitor = config["read_hover"] if self.logHover != None: if self.hover_monitor: self.logHover.start() rospy.loginfo("Hover Logging started") else: self.logHover.stop() rospy.loginfo("Hover Logging stopped") if self.acc_monitor != config["read_acc"]: self.acc_monitor = config["read_acc"] if self.logGyro != None: if self.acc_monitor: self.logAcc.start() rospy.loginfo("Acc Logging started") else: self.logAcc.stop() rospy.loginfo("Acc Logging stopped") if self.baro_monitor != config["read_baro"]: self.baro_monitor = config["read_baro"] if self.logBaro != None: if self.baro_monitor: self.logBaro.start() rospy.loginfo("Baro Logging started") else: self.logBaro.stop() rospy.loginfo("Baro Logging stopped") if self.mag_monitor != config["read_mag"]: self.mag_monitor = config["read_mag"] if self.logMag != None: if self.mag_monitor: self.logMag.start() rospy.loginfo("Mag Logging started") else: self.logMag.stop() rospy.loginfo("Mag Logging stopped") if self.bat_monitor != config["read_bat"]: self.bat_monitor = config["read_bat"] if self.logBat != None: if self.bat_monitor: self.logBat.start() rospy.loginfo("Battery/Link Logging started") else: self.logBat.stop() rospy.loginfo("Battery/Link Logging stopped") if self.attitude_monitor != config["read_attitude"]: self.attitude_monitor = config["read_attitude"] if self.logAttitude != None: if self.attitude_monitor: self.logAttitude.start() rospy.loginfo("Attitude Logging started") else: self.logAttitude.stop() rospy.loginfo("Attitude Logging stopped") if self.motor_monitor != config["read_motor"]: self.motor_monitor = config["read_motor"] if self.logMotor != None: if self.motor_monitor: self.logMotor.start() rospy.loginfo("Motor Logging started") else: self.logMotor.stop() rospy.loginfo("Motor Logging stopped") # SET CRAZYFLIE PARAMS for key, value in config.iteritems(): # Skip this key (whats its purpose??) if key == "groups": continue # Continue if variable exists and is correct if self.cf_params_cache.has_key(key): if config[key] == self.cf_params_cache[key]: # Nothing changed if not key.startswith("magCalib") or ( self.preMagcalib or not config["sensorfusion6_magImu"]): continue # Doesnt exist or has changed # Split into group and name s = key.split("_",1) # Make sure it is a group name pair if len(s) == 2: # make sure group is valid if s[0] in self.cf_param_groups: # update cache, send variable to flie self.cf_params_cache[key] = value self.send_param(s[0]+"."+s[1], value) print "updated: ",s[0]+"."+s[1],"->",value rospy.sleep(0.1) self.preMagcalib = config["sensorfusion6_magImu"] return config def send_param(self, key, value): self.crazyflie.param.set_value(key, str(value)) def restart(self): rospy.loginfo("Restarting Driver") rospy.sleep(1) self.connect(self.options) def logErrorCB(self, errmsg): rospy.logerr("Log error: %s", errmsg) def send_control(self,cmd): """ Roll, pitch in deg, yaw in deg/s, thrust in 10000-60000, hover bool, set_hover bool, hover chance float -1 to +1 """ roll, pitch, yawrate, thrust, hover, set_hover, hover_change = cmd if self.crazyflie.state == CFState.CONNECTED: self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust, hover, set_hover, hover_change) def setup_log(self): """ Console callbacks """ self.crazyflie.console.receivedChar.add_callback(self.consoleCB) rospy.sleep(0.25) """ ATTITUDE LOGGING @ 100hz """ logconf = LogConfig("attitude", self.HZ100 * 2) #ms logconf.addVariable(LogVariable("attitude.q0", "float")) logconf.addVariable(LogVariable("attitude.q1", "float")) logconf.addVariable(LogVariable("attitude.q2", "float")) logconf.addVariable(LogVariable("attitude.q3", "float")) self.logAttitude = self.crazyflie.log.create_log_packet(logconf) if (self.logAttitude is not None): self.logAttitude.dataReceived.add_callback(self.logCallbackAttitude) self.logAttitude.error.add_callback(self.logErrorCB) if self.attitude_monitor: self.logAttitude.start() rospy.loginfo("Attitude Logging started") else: rospy.logwarn("Could not setup Attitude logging!") rospy.sleep(0.25) """ ZPID LOGGING @ 100hz """ logconf = LogConfig("zpid", self.HZ100) #ms logconf.addVariable(LogVariable("zpid.p", "float")) logconf.addVariable(LogVariable("zpid.i", "float")) logconf.addVariable(LogVariable("zpid.d", "float")) logconf.addVariable(LogVariable("zpid.pid", "float")) self.logZPid = self.crazyflie.log.create_log_packet(logconf) if (self.logZPid is not None): self.logZPid.dataReceived.add_callback(self.logCallbackZPid) self.logZPid.error.add_callback(self.logErrorCB) if self.zpid_monitor: self.logZPid.start() rospy.loginfo("ZPID Logging started") else: rospy.logwarn("Could not setup ZPID logging!") rospy.sleep(0.25) """ HOVER LOGGING @ 100hz """ logconf = LogConfig("hover", self.HZ100) #ms logconf.addVariable(LogVariable("hover.err", "float")) logconf.addVariable(LogVariable("hover.target", "float")) logconf.addVariable(LogVariable("hover.zSpeed", "float")) logconf.addVariable(LogVariable("hover.zBias", "float")) logconf.addVariable(LogVariable("hover.acc_vspeed", "float")) logconf.addVariable(LogVariable("hover.asl_vspeed", "float")) self.logHover = self.crazyflie.log.create_log_packet(logconf) if (self.logHover is not None): self.logHover.dataReceived.add_callback(self.logCallbackHover) self.logHover.error.add_callback(self.logErrorCB) if self.hover_monitor: self.logHover.start() rospy.loginfo("Hover Logging started") else: rospy.logwarn("Could not setup Hover logging!") rospy.sleep(0.25) """ GYROMETER LOGGING @ 100hz """ logconf = LogConfig("LoggingGyro", self.HZ100) #ms logconf.addVariable(LogVariable("gyro.x", "float")) logconf.addVariable(LogVariable("gyro.y", "float")) logconf.addVariable(LogVariable("gyro.z", "float")) self.logGyro = self.crazyflie.log.create_log_packet(logconf) if (self.logGyro is not None): self.logGyro.dataReceived.add_callback(self.logCallbackGyro) self.logGyro.error.add_callback(self.logErrorCB) if self.gyro_monitor: self.logGyro.start() rospy.loginfo("Gyro Logging started") else: rospy.logwarn("Could not setup Gyro logging!") rospy.sleep(0.25) """ ACCELEROMETER LOGGING @ 100hz """ logconf = LogConfig("LoggingAcc", self.HZ100) #ms logconf.addVariable(LogVariable("acc.x", "float")) logconf.addVariable(LogVariable("acc.y", "float")) logconf.addVariable(LogVariable("acc.z", "float")) logconf.addVariable(LogVariable("acc.xw", "float")) logconf.addVariable(LogVariable("acc.yw", "float")) logconf.addVariable(LogVariable("acc.zw", "float")) self.logAcc = self.crazyflie.log.create_log_packet(logconf) if (self.logAcc is not None): self.logAcc.dataReceived.add_callback(self.logCallbackAcc) self.logAcc.error.add_callback(self.logErrorCB) if self.acc_monitor: self.logAcc.start() rospy.loginfo("Acc Logging started") else: rospy.logwarn("Could not setup Acc logging!") rospy.sleep(0.25) """ MAGNETOMETER LOGGING @ 100hz """ logconf = LogConfig("LoggingMag", self.HZ100) #ms logconf.addVariable(LogVariable("mag.x", "float")) logconf.addVariable(LogVariable("mag.y", "float")) logconf.addVariable(LogVariable("mag.z", "float")) logconf.addVariable(LogVariable("mag.x_raw", "float")) logconf.addVariable(LogVariable("mag.y_raw", "float")) logconf.addVariable(LogVariable("mag.z_raw", "float")) self.logMag = self.crazyflie.log.create_log_packet(logconf) if (self.logMag is not None): self.logMag.dataReceived.add_callback(self.logCallbackMag) self.logMag.error.add_callback(self.logErrorCB) if self.mag_monitor: self.logMag.start() rospy.loginfo("Mag Logging started") else: rospy.logwarn("Could not setup Mag logging!") rospy.sleep(0.25) """ BAROMETER LOGGING @ 100hz """ logconf = LogConfig("LoggingBaro", self.HZ100) #ms logconf.addVariable(LogVariable("baro.aslRaw", "float")) logconf.addVariable(LogVariable("baro.asl", "float")) logconf.addVariable(LogVariable("baro.temp", "float")) logconf.addVariable(LogVariable("baro.pressure", "float")) logconf.addVariable(LogVariable("baro.aslLong", "float")) self.logBaro = self.crazyflie.log.create_log_packet(logconf) if (self.logBaro is not None): self.logBaro.dataReceived.add_callback(self.logCallbackBaro) self.logBaro.error.add_callback(self.logErrorCB) if self.baro_monitor: self.logBaro.start() rospy.loginfo("Baro Logging started") else: rospy.logwarn("Could not setup Baro logging!") rospy.sleep(0.25) """ BATTERY/LINK LOGGING @ 10hz """ logconf = LogConfig("LoggingBat", self.HZ10) #ms logconf.addVariable(LogVariable("pm.vbat", "float")) logconf.addVariable(LogVariable("pm.state", "uint8_t")) logconf.addVariable(LogVariable("pm.state_charge", "uint8_t")) self.logBat = self.crazyflie.log.create_log_packet(logconf) if (self.logBat is not None): self.logBat.dataReceived.add_callback(self.logCallbackBat) self.logBat.error.add_callback(self.logErrorCB) if self.bat_monitor: self.logBat.start() rospy.loginfo("Bat Logging started") else: rospy.logwarn("Could not setup Battery/Link logging!") rospy.sleep(0.25) #TODO motor logging not working """ MOTOR LOGGING @ 100hz""" logconf = LogConfig("LoggingMotor", self.HZ100) #ms logconf.addVariable(LogVariable("motor.m1", "uint32_t")) logconf.addVariable(LogVariable("motor.m2", "uint32_t")) logconf.addVariable(LogVariable("motor.m3", "uint32_t")) logconf.addVariable(LogVariable("motor.m4", "uint32_t")) logconf.addVariable(LogVariable("motor.thrust", "uint16_t")) logconf.addVariable(LogVariable("motor.vLong", "float")) #TODO move self.logMotor = self.crazyflie.log.create_log_packet(logconf) if (self.logMotor is not None): self.logMotor.dataReceived.add_callback(self.logCallbackMotor) self.logMotor.error.add_callback(self.logErrorCB) if self.motor_monitor: self.logMotor.start() rospy.loginfo("Motor Logging started") else: rospy.logwarn("Could not setup Motor logging!") def logCallbackAcc(self, data): """Accelerometer axis data in mG --> G""" msg = accMSG() msg.header.stamp = rospy.Time.now() msg.acc[0] = data["acc.x"] msg.acc[1] = data["acc.y"] msg.acc[2] = data["acc.z"] msg.acc_w[0] = data["acc.xw"] msg.acc_w[1] = data["acc.yw"] msg.acc_w[2] = data["acc.zw"] self.zspeed.add(msg.acc_w[2]) self.pub_acc.publish(msg) self.pub_tf.sendTransform(msg.acc_w,(0,0,0,1), rospy.Time.now(), "/acc_w","/cf_q") self.pub_tf.sendTransform(msg.acc,(0,0,0,1), rospy.Time.now(), "/acc", "/cf_q") def logCallbackMag(self, data): """TODO UNITS""" msg = magMSG() msg.header.stamp = rospy.Time.now() msg.mag[0] = data["mag.x"] msg.mag[1] = data["mag.y"] msg.mag[2] = data["mag.z"] msg.magRaw[0] = data["mag.x_raw"] msg.magRaw[1] = data["mag.y_raw"] msg.magRaw[2] = data["mag.z_raw"] self.pub_mag.publish(msg) if np.linalg.norm(msg.magRaw)>1: self.pub_tf.sendTransform(msg.mag/np.linalg.norm(msg.magRaw),(0,0,0,1), rospy.Time.now(), "/mag_raw", "/cf_q") if np.linalg.norm(msg.mag)>1: self.pub_tf.sendTransform(msg.mag/np.linalg.norm(msg.mag),(0,0,0,1), rospy.Time.now(), "/mag_calib", "/cf_q") def logCallbackGyro(self, data): """Gyro axis data in deg/s -> rad/s""" msg = gyroMSG() msg.header.stamp = rospy.Time.now() msg.gyro[0] = radians(data["gyro.x"]) msg.gyro[1] = radians(data["gyro.y"]) msg.gyro[2] = radians(data["gyro.z"]) self.pub_gyro.publish(msg) def logCallbackHover(self, data): """Output data regarding hovering""" msg = hoverMSG() msg.header.stamp = rospy.Time.now() msg.err = deadband(data["hover.err"], self.deadband) msg.zSpeed = data["hover.zSpeed"] msg.acc_vspeed = data["hover.acc_vspeed"]# * self.cf_params_cache["hover_acc_vspeedFac"] msg.asl_vspeed = data["hover.asl_vspeed"]# * self.cf_params_cache["hover_asl_vspeedFac"] msg.target = data["hover.target"] msg.zBias = data["hover.zBias"] self.pub_hover.publish(msg) def logCallbackZPid(self, data): msg = zpidMSG() msg.header.stamp = rospy.Time.now() msg.p = data["zpid.p"] msg.i = data["zpid.i"] msg.d = data["zpid.d"] msg.pid = data["zpid.pid"] self.pub_zpid.publish(msg) def logCallbackAttitude(self, data): msg = attitudeMSG() msg.header.stamp = rospy.Time.now() q0 = data["attitude.q0"] q1 = data["attitude.q1"] q2 = data["attitude.q2"] q3 = data["attitude.q3"] msg.q = np.array((q0,q1,q2,q3)) self.pub_attitude.publish(msg) self.pub_tf.sendTransform((0, 0, 0),(q1,q2,q3,q0), rospy.Time.now(), "/cf_q", "/world") def logCallbackGravOffset(self, data): pass def logCallbackBaro(self, data): msg = baroMSG() msg.header.stamp = rospy.Time.now() msg.temp = data["baro.temp"] msg.pressure = data["baro.pressure"] msg.asl = data["baro.asl"] msg.aslRaw = data["baro.aslRaw"] msg.aslLong = data["baro.aslLong"] self.pub_baro.publish(msg) #if self.baro_start_time == None: # self.baro_start_time = rospy.Time.now() #self.csvwriter.writerow([(msg.header.stamp-self.baro_start_time).to_sec(),msg.asl, msg.asl_raw,msg.temperature,msg.pressure]) def logCallbackBat(self, data): msg = batMSG() msg.header.stamp = rospy.Time.now() msg.link = self.link msg.bat_v = data["pm.vbat"] msg.bat_p = (data["pm.vbat"]-3.0)/1.15*100. msg.charge_state = data["pm.state_charge"] msg.state = data["pm.state"] self.pub_bat.publish(msg) self.battery_monitor.update(msg.state, msg.charge_state, msg.bat_v) def logCallbackMotor(self, data): msg = motorMSG() msg.header.stamp = rospy.Time.now() msg.pwm[0] = thrustToPercentage(data["motor.m1"]) msg.pwm[1] = thrustToPercentage(data["motor.m2"]) msg.pwm[2] = thrustToPercentage(data["motor.m3"]) msg.pwm[3] = thrustToPercentage(data["motor.m4"]) msg.thrust = thrustToPercentage(data["motor.thrust"])/100. msg.thrust_raw = data["motor.thrust"] msg.vLong = data["motor.vLong"] self.pub_motor.publish(msg) def connectSetupFinishedCB(self, uri=None): rospy.loginfo("...Connected") self.setup_log() def connectionFailedCB(self, msg=None, errmsg=None): rospy.logerr("Connection failed: %s", errmsg) exit() #self.restart() def connectionInitiatedCB(self, msg=None): rospy.loginfo("Connecting to: %s...", msg) def disconnectedCB(self, msg=None): self.battery_monitor.done() if msg!=None: rospy.loginfo("Disconnected from: %s", msg) def connectionLostCB(self, msg=None, errmsg=None): rospy.logerr("Connection lost: %s", errmsg) self.restart() def linkQualityCB(self, msg=None): self.link = msg def consoleCB(self, msg): rospy.loginfo(msg) def shutdown(self): self.crazyflie.close_link()
def __init__(self): rospy.init_node('mybot_controller') self.dyn_reconf_server = Server(ConfigType, self.reconfigure) self.rate = rospy.get_param('~rate', 50) self.Kp = 0 self.Ki = 0.0 self.Kd = 0 # approx 5.34 rad / s when motor_cmd = 255 self.motor_max_angular_vel = rospy.get_param('~motor_max_angular_vel', 3.97) self.motor_min_angular_vel = rospy.get_param('~motor_min_angular_vel', 1.41) self.wheel_radius = rospy.get_param('~wheel_radius', 0.1) # Corresponding motor commands self.motor_cmd_max = rospy.get_param('~motor_cmd_max', 255) self.motor_cmd_min = rospy.get_param('~motor_cmd_min', 100) # # Publish the computed angular velocity motor command # self.lwheel_angular_vel_motor_pub = rospy.Publisher('lwheel_angular_vel_motor', Float32, queue_size=10) # self.rwheel_angular_vel_motor_pub = rospy.Publisher('rwheel_angular_vel_motor', Float32, queue_size=10) # Publish motor command self.lwheel_motor_cmd_pub = rospy.Publisher('lwheel_motor_cmd_pub', Int32, queue_size=10) self.rwheel_motor_cmd_pub = rospy.Publisher('rwheel_motor_cmd_pub', Int32, queue_size=10) self.param_pid = rospy.Subscriber('param_pid', String, self.param_pid_callback) # Read in encoders for PID control self.lwheel_angular_vel_enc_sub = rospy.Subscriber( 'lwheel_angular_vel_enc', Float32, self.lwheel_angular_vel_enc_callback) self.rwheel_angular_vel_enc_sub = rospy.Subscriber( 'rwheel_angular_vel_enc', Float32, self.rwheel_angular_vel_enc_callback) # Read in tangential velocity targets self.lwheel_tangent_vel_target_sub = rospy.Subscriber( 'lwheel_tangent_vel_target', Float32, self.lwheel_tangent_vel_target_callback) self.rwheel_tangent_vel_target_sub = rospy.Subscriber( 'rwheel_tangent_vel_target', Float32, self.rwheel_tangent_vel_target_callback) # Tangential velocity target self.lwheel_tangent_vel_target = 0 self.rwheel_tangent_vel_target = 0 # Angular velocity target self.lwheel_angular_vel_target = 0 self.rwheel_angular_vel_target = 0 # Angular velocity encoder readings self.lwheel_angular_vel_enc = 0 self.rwheel_angular_vel_enc = 0 # Value motor command self.lwheel_motor_cmd = 0 self.rwheel_motor_cmd = 0 # PID control variables self.lwheel_pid = {} self.rwheel_pid = {}
def __init__(self): logger.info('Starting performances node') self.robot_name = rospy.get_param('/robot_name') self.robots_config_dir = rospy.get_param('/robots_config_dir') self.running = False self.paused = False self.autopause = False self.pause_time = 0 self.start_time = 0 self.start_timestamp = 0 self.lock = Lock() self.run_condition = Condition() self.running_performance = None self.unload_finished = False # in memory set of properties with priority over params self.variables = {} # References to event subscribing node callbacks self.observers = {} # Performances that already played as alternatives. Used to maximize different performance in single demo self.performances_played = {} self.worker = Thread(target=self.worker) self.worker.setDaemon(True) rospy.init_node('performances') self.services = { 'head_pau_mux': rospy.ServiceProxy('/' + self.robot_name + '/head_pau_mux/select', MuxSelect), 'neck_pau_mux': rospy.ServiceProxy('/' + self.robot_name + '/neck_pau_mux/select', MuxSelect) } self.topics = { 'running_performance': rospy.Publisher('~running_performance', String, queue_size=1), 'look_at': rospy.Publisher('/blender_api/set_face_target', Target, queue_size=1), 'gaze_at': rospy.Publisher('/blender_api/set_gaze_target', Target, queue_size=1), 'head_rotation': rospy.Publisher('/blender_api/set_head_rotation', Float32, queue_size=1), 'emotion': rospy.Publisher('/blender_api/set_emotion_state', EmotionState, queue_size=3), 'gesture': rospy.Publisher('/blender_api/set_gesture', SetGesture, queue_size=3), 'expression': rospy.Publisher('/' + self.robot_name + '/make_face_expr', MakeFaceExpr, queue_size=3), 'kfanimation': rospy.Publisher('/' + self.robot_name + '/play_animation', PlayAnimation, queue_size=3), 'interaction': rospy.Publisher('/behavior_switch', String, queue_size=1), 'bt_control': rospy.Publisher('/behavior_control', Int32, queue_size=1), 'events': rospy.Publisher('~events', Event, queue_size=1), 'chatbot': rospy.Publisher('/' + self.robot_name + '/speech', ChatMessage, queue_size=1), 'speech_events': rospy.Publisher('/' + self.robot_name + '/speech_events', String, queue_size=1), 'soma_state': rospy.Publisher("/blender_api/set_soma_state", SomaState, queue_size=2), 'tts': { 'en': rospy.Publisher('/' + self.robot_name + '/tts_en', String, queue_size=1), 'zh': rospy.Publisher('/' + self.robot_name + '/tts_zh', String, queue_size=1), 'default': rospy.Publisher('/' + self.robot_name + '/tts', String, queue_size=1), }, 'tts_control': rospy.Publisher('/' + self.robot_name + '/tts_control', String, queue_size=1) } self.load_properties() rospy.Service('~reload_properties', Trigger, self.reload_properties_callback) rospy.Service('~set_properties', srv.SetProperties, self.set_properties_callback) rospy.Service('~load', srv.Load, self.load_callback) rospy.Service('~load_performance', srv.LoadPerformance, self.load_performance_callback) rospy.Service('~unload', Trigger, self.unload_callback) rospy.Service('~run', srv.Run, self.run_callback) rospy.Service('~run_by_name', srv.RunByName, self.run_by_name_callback) rospy.Service('~run_full_performance', srv.RunByName, self.run_full_performance_callback) rospy.Service('~resume', srv.Resume, self.resume_callback) rospy.Service('~pause', srv.Pause, self.pause_callback) rospy.Service('~stop', srv.Stop, self.stop) rospy.Service('~current', srv.Current, self.current_callback) # Shared subscribers for nodes rospy.Subscriber('/' + self.robot_name + '/speech_events', String, lambda msg: self.notify('speech_events', msg)) rospy.Subscriber('/' + self.robot_name + '/speech', ChatMessage, self.speech_callback) # Shared subscribers for nodes rospy.Subscriber('/hand_events', String, self.hand_callback) Server(PerformancesConfig, self.reconfig) rospy.Subscriber('/face_training_event', String, self.training_callback) self.worker.start() rospy.spin()
# ------------------- end of callback ---------------- # this runs when the button is clicked in Rviz - Currently doesn't do a lot def callbackrviz(data): global flag_first, flag_end, n_goals if goal_array.shape[0] != n_goals + 1: flag_first = True # sets the flag when rviz nav goal button clicked if __name__ == '__main__': rospy.init_node('goal_selector', anonymous=True) # initialise node "move_mallard" # pub_goal = rospy.Publisher('/mallard/goals',PoseStamped,queue_size=10) pub_goal = rospy.Publisher('/mallard/goals', Float64MultiArray, queue_size=10) rospy.Subscriber("/slam_out_pose", PoseStamped, slam_callback, param) # subscribes to topic "/slam_out_pose" rospy.Subscriber('/path_poses', PoseArray, path_callback, queue_size=1) # Gets new sets of goals rospy.Subscriber("/move_base_simple/goal", PoseStamped, callbackrviz) # Subscribe to array of goal poses from RVIZ interactive coverage selector dynrecon = Server(MtwoParamConfig, dynReconfigCallback) rospy.spin() # -------------------------------------------------------------------------------- # data_to_send = Float64MultiArray() # the data to be sent, initialise the array # data_to_send.data = array # assign the array with the value you want to send # pub.publish(data_to_send)u
def __init__(self): rospy.init_node('turtlebot_navigator', anonymous=True) self.mode = Mode.IDLE self.xlist = [] self.ylist = [] self.tlist = [] self.object_dic = {} with open('locations.json', 'r') as f: self.object_dic = json.load(f) for i in self.object_dic: print i cal_points(self.object_dic) print(point_mat) # current state self.x = 0.0 self.y = 0.0 self.theta = 0.0 # goal state self.x_g = None self.y_g = None self.theta_g = None self.th_init = 0.0 # map parameters self.map_width = 0 self.map_height = 0 self.map_resolution = 0 self.map_origin = [0, 0] self.map_probs = [] self.occupancy = None self.occupancy_updated = False # plan parameters self.plan_resolution = 0.1 self.plan_horizon = 15 # time when we started following the plan self.current_plan_start_time = rospy.get_rostime() self.current_plan_duration = 0 self.plan_start = [0., 0.] # Robot limits self.v_max = 0.2 self.om_max = 0.5 #0.5 self.v_des = 0.12 # desired cruising velocity self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it # threshold at which navigator switches from trajectory to pose control self.near_thresh = 0.2 self.at_thresh = 0.02 self.at_thresh_theta = 0.05 # trajectory smoothing self.spline_alpha = 0.15 self.traj_dt = 0.1 # trajectory tracking controller parameters self.kpx = 0.5 self.kpy = 0.5 self.kdx = 1.5 self.kdy = 1.5 #try self.kpx = 0.5 self.kpy = 0.5 self.kdx = 1.5 self.kdy = 1.5 # heading controller parameters self.kp_th = 2. self.kp_th = 5. #try self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max) self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max) self.heading_controller = HeadingController(self.kp_th, self.om_max) self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10) self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10) self.nav_smoothed_path_rej_pub = rospy.Publisher( '/cmd_smoothed_path_rejected', Path, queue_size=10) self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) self.vis_pub = rospy.Publisher('marker_topic', Marker, queue_size=10) self.trans_listener = tf.TransformListener() self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback) self.stop_min_dist = 1.0 self.stop_time = 3.0 self.crossing_time = 10.0 rospy.Subscriber('/map', OccupancyGrid, self.map_callback) rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback) rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback) rospy.Subscriber('/detector/stop_sign', DetectedObject, self.stop_sign_detected_callback) rospy.Subscriber('/detector/broccoli', DetectedObject, self.broccoli_callback) rospy.Subscriber('/detector/cake', DetectedObject, self.cake_callback) rospy.Subscriber('/detector/bowl', DetectedObject, self.bowl_callback) rospy.Subscriber('/detector/banana', DetectedObject, self.banana_callback) rospy.Subscriber('/detector/donut', DetectedObject, self.donut_callback) rospy.Subscriber('/delivery_request', String, self.delivery_callback) self.initPos = False self.auto = False self.broccoli = 0 self.cake = 1 self.bowl = 2 self.banana = 3 self.donut = 4 self.control = False print "finished init"
def __init__(self): rospy.init_node('turtlebot_navigator', anonymous=True) self.mode = Mode.IDLE self.mode_at_stop = None self.x_saved = None self.y_saved = None self.theta_saved = None # current state self.x = 0.0 self.y = 0.0 self.theta = 0.0 # history tracking of controls self.history_cnt = 0 self.V_history = np.zeros(CMD_HISTORY_SIZE) self.om_history = np.zeros(CMD_HISTORY_SIZE) self.backing_cnt = 0 #laser scans for collision self.laser_ranges = [] self.laser_angle_increment = 0.01 # this gets updated self.chunky_radius = 0.11 # goal state self.x_g = None self.y_g = None self.theta_g = None self.stop_flag = False self.th_init = 0.0 self.iters = 0 # map parameters self.map_width = 0 self.map_height = 0 self.map_resolution = 0 self.map_origin = [0, 0] self.map_probs = [] self.map_probs_inflated = [] self.occupancy = None self.occupancy_updated = False # plan parameters self.plan_resolution = 0.1 self.plan_horizon = 15 # time when we started following the plan self.current_plan_start_time = rospy.get_rostime() self.current_plan_duration = 0 self.plan_start = [0., 0.] # Robot limits self.v_max = rospy.get_param("~v_max", 0.2) #0.2 # maximum velocity self.om_max = rospy.get_param("~om_max", 0.4) #0.4 # maximum angular velocity self.v_des = 0.12 # desired cruising velocity self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it # threshold at which navigator switches from trajectory to pose control self.near_thresh = 0.2 self.at_thresh = 0.02 self.at_thresh_theta = (2.0 * np.pi) / 20.0 #0.05 # trajectory smoothing self.spline_alpha = 0.01 self.traj_dt = 0.1 # trajectory tracking controller parameters self.kpx = 0.5 self.kpy = 0.5 self.kdx = 1.5 self.kdy = 1.5 # heading controller parameters self.kp_th = 2. self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max) self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max) self.heading_controller = HeadingController(self.kp_th, self.om_max) # timing variables self.start_time = 0.0 self.wait_time = None # Publishers self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10) self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10) self.nav_smoothed_path_rej_pub = rospy.Publisher( '/cmd_smoothed_path_rejected', Path, queue_size=10) self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) self.trans_listener = tf.TransformListener() self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback) #communication with squirtle_fsm to inform goal status self.publish_squirtle = rospy.Publisher('/post/squirtle_fsm', String, queue_size=10) #Publisher for our state self.nav_mode_publisher = rospy.Publisher('/state_bd/nav_fsm', String, queue_size=5) # Subscriber Constructors rospy.Subscriber('/post/nav_fsm', Bool, self.post_callback) #service queue rospy.Subscriber('/map', OccupancyGrid, self.map_callback) rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback) rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback) rospy.Subscriber('/scan', LaserScan, self.laser_callback) rospy.Subscriber('debug/nav_fsm', String, self.debug_callback) print "finished init"
def __init__(self): """ Variables to track communication frequency for debugging """ self.summer=0 self.samp = 0 self.avg_freq = 0.0 self.start_frequency_samp = False self.need_to_terminate = False self.flush_rcvd_data=True self.update_base_local_planner = False self.parameter_server_is_updating = False self.last_move_base_update = rospy.Time.now().to_sec() """ Initialize the publishers for VECTOR """ self.vector_data = VECTOR_DATA() """ Start the thread for the linear actuator commands """ self._linear = LinearActuator() if (False == self._linear.init_success): rospy.logerr("Could not initialize the linear actuator interface! exiting...") return """ Initialize faultlog related items """ self.is_init = True self.extracting_faultlog = False """ Initialize the dynamic reconfigure server for VECTOR """ self.param_server_initialized = False self.dyn_reconfigure_srv = Server(vectorConfig, self._dyn_reconfig_callback) """ Wait for the parameter server to set the configs and then set the IP address from that. Note that this must be the current ethernet settings of the platform. If you want to change it set the ethernet settings at launch to the current ethernet settings, power up, change them, power down, set the the ethernet settings at launch to the new ones and relaunch """ r = rospy.Rate(10) start_time = rospy.Time.now().to_sec() while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == self.param_server_initialized): r.sleep() if (False == self.param_server_initialized): rospy.logerr("Parameter server not found, you must pass an initial yaml in the launch! exiting...") return """ Create the thread to run VECTOR communication """ self.tx_queue_ = multiprocessing.Queue() self.rx_queue_ = multiprocessing.Queue() self.comm = IoEthThread((os.environ['VECTOR_IP_ADDRESS'],int(os.environ['VECTOR_IP_PORT_NUM'])), self.tx_queue_, self.rx_queue_, max_packet_size=1248) if (False == self.comm.link_up): rospy.logerr("Could not open socket for VECTOR...") self.comm.close() return """ Initialize the publishers and subscribers for the node """ self.faultlog_pub = rospy.Publisher('/vector/feedback/faultlog', Faultlog, queue_size=10,latch=True) rospy.Subscriber("/vector/cmd_vel", Twist, self._add_motion_command_to_queue) rospy.Subscriber("/vector/gp_command",ConfigCmd,self._add_config_command_to_queue) rospy.Subscriber("/move_base/DWAPlannerROS/parameter_updates",Config,self._update_move_base_params) """ Start the receive handler thread """ self.terminate_mutex = threading.RLock() self.last_rsp_rcvd = rospy.Time.now().to_sec() self._rcv_thread = threading.Thread(target = self._run) self._rcv_thread.start() """ Start streaming continuous data """ rospy.loginfo("Stopping the data stream") if (False == self._continuous_data(False)): rospy.logerr("Could not stop VECTOR communication stream") self.Shutdown() return """ Extract the faultlog at startup """ self.flush_rcvd_data=False rospy.loginfo("Extracting the faultlog") self.extracting_faultlog = True if (False == self._extract_faultlog()): rospy.logerr("Could not get retrieve VECTOR faultlog") self.Shutdown() return """ Start streaming continuous data """ rospy.loginfo("Starting the data stream") if (False == self._continuous_data(True)): rospy.logerr("Could not start VECTOR communication stream") self.Shutdown() return self.start_frequency_samp = True """ Force the configuration to update the first time to ensure that the variables are set to the correct values on the machine """ if (False == self._force_machine_configuration(True)): rospy.logerr("Initial configuration parameteters my not be valid, please check them in the yaml file") rospy.logerr("The ethernet address must be set to the present address at startup, to change it:") rospy.logerr("start the machine; change the address using rqt_reconfigure; shutdown; update the yaml and restart") self.Shutdown() return """ Uncomment the line below to always unlock gain tuning and force the upload based on the parameters in the yaml file you must have the key to do this because it can be dangerous if one sets unstable gains. 0x00000000 needs to be replaced by the key """ #self._unlock_gain_tuning(0x00000000) #self._force_pid_configuration(True) """ Finally update the values for dynamic reconfigure with the ones reported by the machine """ new_config = dict({"x_vel_limit_mps" : self.vector_data.config_param.machcfg.x_vel_limit_mps, "y_vel_limit_mps" : self.vector_data.config_param.machcfg.y_vel_limit_mps, "accel_limit_mps2" : self.vector_data.config_param.machcfg.accel_limit_mps2, "dtz_decel_limit_mps2": self.vector_data.config_param.machcfg.dtz_decel_limit_mps2, "yaw_rate_limit_rps" : self.vector_data.config_param.machcfg.yaw_rate_limit_rps, "wheel_diameter_m": self.vector_data.config_param.machcfg.wheel_diameter_m, "wheel_base_length_m": self.vector_data.config_param.machcfg.wheelbase_length_m, "wheel_track_width_m": self.vector_data.config_param.machcfg.wheel_track_width_m, "gear_ratio" : self.vector_data.config_param.machcfg.gear_ratio, "motion_while_charging" : ((self.vector_data.config_param.machcfg.config_bitmap >> DISABLE_AC_PRESENT_CSI_SHIFT) & 1) ^ 1, "motion_ctl_input_filter" : (self.vector_data.config_param.machcfg.config_bitmap >> MOTION_MAPPING_FILTER_SHIFT) & VALID_MOTION_MAPPING_FILTER_MASK, "p_gain_rps_per_rps" : self.vector_data.config_param.ctlconfig.p_gain_rps_per_rps, "i_gain_rps_per_rad" : self.vector_data.config_param.ctlconfig.i_gain_rps_per_rad, "d_gain_rps_per_rps2" : self.vector_data.config_param.ctlconfig.d_gain_rps_per_rps2, "fdfwd_gain_rps_per_motor_rps" : self.vector_data.config_param.ctlconfig.fdfwd_gain_rps_per_motor_rps, "p_error_limit_rps" : self.vector_data.config_param.ctlconfig.p_error_limit_rps, "i_error_limit_rad" : self.vector_data.config_param.ctlconfig.i_error_limit_rad, "i_error_drain_rate_rad_per_frame" : self.vector_data.config_param.ctlconfig.i_error_drain_rate_rad_per_frame, "input_target_limit_rps" : self.vector_data.config_param.ctlconfig.input_target_limit_rps, "output_limit_rps" : self.vector_data.config_param.ctlconfig.input_target_limit_rps}) self.dyn_reconfigure_srv.update_configuration(new_config) rospy.loginfo("Vector Driver is up and running")
def __init__(self): # current state self.x = 0.0 self.y = 0.0 self.theta = 0.0 # goal state self.x_g = None self.y_g = None self.theta_g = None self.th_init = 0.0 # map parameters self.map_width = 0 self.map_height = 0 self.map_resolution = 0 self.map_origin = [0, 0] self.map_probs = [] self.occupancy = None self.occupancy_updated = False # plan parameters self.plan_resolution = 0.1 self.plan_horizon = 15 # time when we started following the plan self.current_plan_start_time = rospy.get_rostime() self.current_plan_duration = 0 self.plan_start = [0., 0.] # Robot limits self.v_max = 0.2 # maximum velocity self.om_max = 0.4 # maximum angular velocity self.v_des = 0.12 # desired cruising velocity self.theta_start_thresh = 0.05 # threshold in theta to start moving forward when path-following self.start_pos_thresh = 0.2 # threshold to be far enough into the plan to recompute it # threshold at which navigator switches from trajectory to pose control self.near_thresh = 0.2 self.at_thresh = 0.02 self.at_thresh_theta = 0.05 # trajectory smoothing self.spline_alpha = 0.15 self.traj_dt = 0.1 # trajectory tracking controller parameters self.kpx = 0.5 self.kpy = 0.5 self.kdx = 1.5 self.kdy = 1.5 # heading controller parameters self.kp_th = 2. self.traj_controller = TrajectoryTracker(self.kpx, self.kpy, self.kdx, self.kdy, self.v_max, self.om_max) self.pose_controller = PoseController(0., 0., 0., self.v_max, self.om_max) self.heading_controller = HeadingController(self.kp_th, self.om_max) self.nav_planned_path_pub = rospy.Publisher('/planned_path', Path, queue_size=10) self.nav_smoothed_path_pub = rospy.Publisher('/cmd_smoothed_path', Path, queue_size=10) self.nav_smoothed_path_rej_pub = rospy.Publisher( '/cmd_smoothed_path_rejected', Path, queue_size=10) self.nav_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) self.trans_listener = tf.TransformListener() self.cfg_srv = Server(NavigatorConfig, self.dyn_cfg_callback) rospy.Subscriber('/map', OccupancyGrid, self.map_callback) rospy.Subscriber('/map_metadata', MapMetaData, self.map_md_callback) rospy.Subscriber('/cmd_nav', Pose2D, self.cmd_nav_callback) self.new_cmd_nav = False self.new_map = False
def main(): rospy.init_node('ur_driver', disable_signals=True) if rospy.get_param("use_sim_time", False): rospy.logwarn("use_sim_time is set!!!") global prevent_programming reconfigure_srv = Server(URDriverConfig, reconfigure_callback) prefix = rospy.get_param("~prefix", "") print "Setting prefix to %s" % prefix global joint_names joint_names = [prefix + name for name in JOINT_NAMES] # Parses command line arguments parser = optparse.OptionParser(usage="usage: %prog robot_hostname [reverse_port]") (options, args) = parser.parse_args(rospy.myargv()[1:]) if len(args) < 1: parser.error("You must specify the robot hostname") elif len(args) == 1: robot_hostname = args[0] reverse_port = DEFAULT_REVERSE_PORT elif len(args) == 2: robot_hostname = args[0] reverse_port = int(args[1]) if not (0 <= reverse_port <= 65535): parser.error("You entered an invalid port number") else: parser.error("Wrong number of parameters") # Reads the calibrated joint offsets from the URDF global joint_offsets joint_offsets = load_joint_offsets(joint_names) if len(joint_offsets) > 0: rospy.loginfo("Loaded calibration offsets from urdf: %s" % joint_offsets) else: rospy.loginfo("No calibration offsets loaded from urdf") # Reads the maximum velocity # The max_velocity parameter is only used for debugging in the ur_driver. It's not related to actual velocity limits global max_velocity max_velocity = rospy.get_param("~max_velocity", MAX_VELOCITY) # [rad/s] rospy.loginfo("Max velocity accepted by ur_driver: %s [rad/s]" % max_velocity) # Reads the minimum payload global min_payload min_payload = rospy.get_param("~min_payload", MIN_PAYLOAD) # Reads the maximum payload global max_payload max_payload = rospy.get_param("~max_payload", MAX_PAYLOAD) rospy.loginfo("Bounds for Payload: [%s, %s]" % (min_payload, max_payload)) # Sets up the server for the robot to connect to server = TCPServer(("", reverse_port), CommanderTCPHandler) thread_commander = threading.Thread(name="CommanderHandler", target=server.serve_forever) thread_commander.daemon = True thread_commander.start() with open(roslib.packages.get_pkg_dir('ur_driver') + '/prog') as fin: program = fin.read() % {"driver_hostname": get_my_ip(robot_hostname, PORT), "driver_reverseport": reverse_port} connection = URConnection(robot_hostname, PORT, program) connection.connect() connection.send_reset_program() connectionRT = URConnectionRT(robot_hostname, RT_PORT) connectionRT.connect() set_io_server() service_provider = None action_server = None try: while not rospy.is_shutdown(): # Checks for disconnect if getConnectedRobot(wait=False): time.sleep(0.2) try: prevent_programming = rospy.get_param("~prevent_programming") update = {'prevent_programming': prevent_programming} reconfigure_srv.update_configuration(update) except KeyError, ex: print "Parameter 'prevent_programming' not set. Value: " + str(prevent_programming) pass if prevent_programming: print "Programming now prevented" connection.send_reset_program() else: print "Disconnected. Reconnecting" if action_server: action_server.set_robot(None) rospy.loginfo("Programming the robot") while True: # Sends the program to the robot while not connection.ready_to_program(): print "Waiting to program" time.sleep(1.0) try: prevent_programming = rospy.get_param("~prevent_programming") update = {'prevent_programming': prevent_programming} reconfigure_srv.update_configuration(update) except KeyError, ex: print "Parameter 'prevent_programming' not set. Value: " + str(prevent_programming) pass connection.send_program() r = getConnectedRobot(wait=True, timeout=1.0) if r: break rospy.loginfo("Robot connected") #provider for service calls if service_provider: service_provider.set_robot(r) else: service_provider = URServiceProvider(r) if action_server: action_server.set_robot(r) else: action_server = URTrajectoryFollower(r, rospy.Duration(1.0)) action_server.start()