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