#!/usr/bin/env python
import rospy
from dynamic_reconfigure.server import Server
from recon_surface.cfg import ReconSurfaceConfig
def callback(config, level):
rospy.loginfo("""Reconfigure Request: {}""".format(config))
return config
if __name__ == "__main__":
rospy.init_node("recon_surface_dyn_server_py", anonymous=False)
srv = Server(ReconSurfaceConfig, callback)
rospy.spin()
#--------------------
# ----------------------------------------------------------------------------
def callbackDynParam(config, level):
# -----------------------------------------------------------------------------
global kAngle
kAngle = float("""{kAngle}""".format(**config))
return config
# server for dyamic parameters
srv = Server(ControlPositionCFGConfig, callbackDynParam)
#init dynamic parameters
kAngle = rospy.get_param('/controlPosition/kAngle', 1.5)
thetaRef = 0.0
# -----------------------------------------------------------------------------
def callBackOdometry(data):
# -----------------------------------------------------------------------------
global robot
# assign robot coordinates
robot.x = data.pose.pose.position.x
robot.y = data.pose.pose.position.y
[rool, pitch, yaw] = tf.transformations.euler_from_quaternion([
#!/usr/bin/env python
import rospy
from dynamic_reconfigure.server import Server
from rtcrobot_driver.cfg import Light_Config
def callback(config, level):
rospy.loginfo("""Reconfigure Request: {state}""".format(**config))
print config.state
return config
if __name__ == "__main__":
rospy.init_node("control_light", anonymous=False)
srv = Server(Light_Config, callback)
rospy.spin()
# Callback for dynamic reconfigure requests
def reconfig_callback(config, level):
global imu_yaw_calibration
rospy.loginfo("""Reconfigure request for yaw_calibration: %d""" %
(config['yaw_calibration']))
#if imu_yaw_calibration != config('yaw_calibration'):
imu_yaw_calibration = config['yaw_calibration']
rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
return config
rospy.init_node("razor_node")
#We only care about the most recent measurement, i.e. queue_size=1
pub = rospy.Publisher('imu', Imu, queue_size=1)
srv = Server(imuConfig,
reconfig_callback) # define dynamic_reconfigure callback
diag_pub = rospy.Publisher('diagnostics', DiagnosticArray, queue_size=1)
diag_pub_time = rospy.get_time()
imuMsg = Imu()
# Orientation covariance estimation:
# Observed orientation noise: 0.3 degrees in x, y, 0.6 degrees in z
# Magnetometer linearity: 0.1% of full scale (+/- 2 gauss) => 4 milligauss
# Earth's magnetic field strength is ~0.5 gauss, so magnetometer nonlinearity could
# cause ~0.8% yaw error (4mgauss/0.5 gauss = 0.008) => 2.8 degrees, or 0.050 radians
# i.e. variance in yaw: 0.0025
# Accelerometer non-linearity: 0.2% of 4G => 0.008G. This could cause
# static roll/pitch error of 0.8%, owing to gravity orientation sensing
# error => 2.8 degrees, or 0.05 radians. i.e. variance in roll/pitch: 0.0025
# so set all covariances the same.
def __init__(self):
# Give the node a name
rospy.init_node('calibrate_linear', anonymous=False)
# Set rospy to exectute a shutdown function when terminating the script
rospy.on_shutdown(self.shutdown)
# How fast will we check the odometry values?
self.rate = 30
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)
# 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()
# 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
# 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 start(self, callback):
self.dyn_rec_srv = Server(self.get_type(),
callback,
namespace=self.name)
yController = LinearController(YPub)
def dvlCb(msg):
xController.updateState(msg.velocity.x)
yController.updateState(msg.velocity.y)
# Publish new forces
XPub.publish(xController.force)
YPub.publish(yController.force)
def dynamicReconfigureCb(config, level):
# On dynamic reconfiguration
xController.reconfigure(config, "x")
yController.reconfigure(config, "y")
return config
if __name__ == '__main__':
rospy.init_node("linear_controller")
# Set subscribers
rospy.Subscriber("/command/x", LinearCommand, xController.cmdCb)
rospy.Subscriber("/command/y", LinearCommand, yController.cmdCb)
rospy.Subscriber("/state/dvl", Dvl, dvlCb)
Server(LinearControllerConfig, dynamicReconfigureCb)
rospy.spin()
def __init__(self):
super(ClassificationResultVisualizer, self).__init__()
self.srv = Server(ClassificationResultVisualizerConfig,
self.config_callback)
self.pub_marker = self.advertise("~output", MarkerArray, queue_size=10)
self.checkpoint_time,
rospy.get_time() - self.checkpoint_time)
return self._drawn_artists
def new_frame_seq(self):
if self.display:
return iter(xrange(sys.maxint))
def _init_draw(self):
if self.display:
lines = [self.line_spike]
for x in range(self.n_joints):
lines.append(self.analog_lines[x])
for l in lines:
l.set_data([], [])
def close_display(self):
self.display = False
Animation = RasterplotAnimation()
srv = Server(DynParametersConfig, Animation.param_callback)
while not rospy.is_shutdown():
if Animation.display:
Animation.new_figure()
matplotlib.pyplot.show()
Animation.close_display()
Animation.end_subscriber()
rospy.sleep(1)
# TODO what happens with malformed target goal??? FAILURE or INVALID_POSE
# txt must be: "Aborting on goal because it was sent with an invalid quaternion"
# move_base_flex get_path and move_base action clients
mbf_mb_ac = actionlib.SimpleActionClient("move_base_flex/move_base",
mbf_msgs.MoveBaseAction)
mbf_gp_ac = actionlib.SimpleActionClient("move_base_flex/get_path",
mbf_msgs.GetPathAction)
mbf_mb_ac.wait_for_server(rospy.Duration(20))
mbf_gp_ac.wait_for_server(rospy.Duration(10))
# move_base_flex dynamic reconfigure client
mbf_drc = Client("move_base_flex", timeout=10)
# move_base simple topic and action server
mb_sg = rospy.Subscriber('move_base_simple/goal', PoseStamped,
simple_goal_cb)
mb_as = actionlib.SimpleActionServer('move_base',
mb_msgs.MoveBaseAction,
mb_execute_cb,
auto_start=False)
mb_as.start()
# move_base make_plan service
mb_mps = rospy.Service('~make_plan', nav_srvs.GetPlan, make_plan_cb)
# move_base dynamic reconfigure server
mb_drs = Server(MoveBaseConfig, mb_reconf_cb)
rospy.spin()
search_distance=config['search_distance']
z_threshold =config['z_threshold']
ver_dist_to_pinger=config['altitude_at_pinger']-0.6+0.13
#1.13: dist from hydrophone to DVL, 0.5, height of pinger
return config
if __name__ == '__main__':
rospy.init_node('acoustic_navigation',log_level=rospy.DEBUG)
loop_rate=rospy.Rate(0.5)
rospy.loginfo("AcousticNavigation activated!")
rospy.wait_for_service('set_controller_srv')
isTest = rospy.get_param('~testmode',False)
#Setup dynamic reconfigure
reconfigure_srv=Server(acoustic_navConfig, dynamic_reconfigure_cb)
#Setup movement client
movement_client = actionlib.SimpleActionClient('LocomotionServer', bbauv_msgs.msg.ControllerAction)
movement_client.wait_for_server()
if isTest:
isStart= True
cur_heading=10000 #dummy val
set_controller_request=rospy.ServiceProxy('set_controller_srv',set_controller)
set_controller_request(True,True,True,True,False,False,False)
else:
#setup client
rospy.loginfo('waiting for mission_srv...')
rospy.wait_for_service('mission_srv') #connecting to mission server
mission_srv_request = rospy.ServiceProxy('mission_srv', vision_to_mission, headers={'id':'6'})
def __init__(self):
self.subscriber = rospy.Subscriber('task1/number', Int16,
self.callback)
self.server = Server(SimpleComParamsConfig, self.callback_dyn)
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(1.1)
self.pid_roll.set_ki(0.2)
self.pid_roll.set_kd(0.0)
self.pid_roll_rate.set_kp(0.6)
self.pid_roll_rate.set_ki(0.4)
self.pid_roll_rate.set_kd(0)
self.pid_roll_rate.set_lim_high(0.25)
self.pid_roll_rate.set_lim_low(-0.25)
self.pid_pitch.set_kp(1.1)
self.pid_pitch.set_ki(0.2)
self.pid_pitch.set_kd(0.0)
self.pid_pitch_rate.set_kp(0.6)
self.pid_pitch_rate.set_ki(0.4)
self.pid_pitch_rate.set_kd(0.0)
self.pid_pitch_rate.set_lim_high(0.25)
self.pid_pitch_rate.set_lim_low(-0.25)
self.pid_yaw.set_kp(1.0)
self.pid_yaw.set_ki(0)
self.pid_yaw.set_kd(0.1)
self.pid_yaw_rate.set_kp(200.0)
self.pid_yaw_rate.set_ki(0)
self.pid_yaw_rate.set_kd(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(400)
self.pid_vpc_roll.set_lim_low(-400)
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(400)
self.pid_vpc_pitch.set_lim_low(-400)
# Filter parameters
self.rate_mv_filt_K = 1.0
self.rate_mv_filt_T = 0.05
# 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(VpcTtcuavAttitudeCtlParamsConfig,
self.cfg_callback)
def __init__(self, img_proc=None):
self.subject_tracker = FaceEncodingTracker() if rospy.get_param(
"~use_face_encoding_tracker",
default=False) else SequentialTracker()
self.bridge = CvBridge()
self.__subject_bridge = SubjectListBridge()
self.model_size_rescale = 30.0
self.head_pitch = 0.0
self.margin = rospy.get_param("~margin", 42)
self.margin_eyes_height = rospy.get_param("~margin_eyes_height", 36)
self.margin_eyes_width = rospy.get_param("~margin_eyes_width", 60)
self.interpupillary_distance = 0.058
self.cropped_face_size = (rospy.get_param("~face_size_height", 224),
rospy.get_param("~face_size_width", 224))
self.device_id_facedetection = rospy.get_param(
"~device_id_facedetection", default="cuda:0")
self.device_id_facealignment = rospy.get_param(
"~device_id_facealignment", default="cuda:0")
rospy.loginfo("Using device {} for face detection.".format(
self.device_id_facedetection))
rospy.loginfo("Using device {} for face alignment.".format(
self.device_id_facealignment))
self.rgb_frame_id = rospy.get_param("~rgb_frame_id", "/kinect2_link")
self.rgb_frame_id_ros = rospy.get_param("~rgb_frame_id_ros",
"/kinect2_nonrotated_link")
self.model_points = None
self.eye_image_size = (rospy.get_param("~eye_image_width", 60),
rospy.get_param("~eye_image_height", 36))
self.tf_broadcaster = TransformBroadcaster()
self.tf_listener = TransformListener()
self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
self.use_previous_headpose_estimate = True
self.last_rvec = {}
self.last_tvec = {}
self.pose_stabilizers = {} # Introduce scalar stabilizers for pose.
try:
if img_proc is None:
tqdm.write("Wait for camera message")
cam_info = rospy.wait_for_message("/camera_info",
CameraInfo,
timeout=None)
self.img_proc = PinholeCameraModel()
# noinspection PyTypeChecker
self.img_proc.fromCameraInfo(cam_info)
else:
self.img_proc = img_proc
if np.array_equal(
self.img_proc.intrinsicMatrix(),
np.matrix([[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]])):
raise Exception(
'Camera matrix is zero-matrix. Did you calibrate'
'the camera and linked to the yaml file in the launch file?'
)
tqdm.write("Camera message received")
except rospy.ROSException:
raise Exception("Could not get camera info")
# multiple person images publication
self.subject_pub = rospy.Publisher("/subjects/images",
MSG_SubjectImagesList,
queue_size=1)
# multiple person faces publication for visualisation
self.subject_faces_pub = rospy.Publisher("/subjects/faces",
Image,
queue_size=1)
self.model_points = self._get_full_model_points()
self.sess_bb = None
self.face_net = FaceDetector(device=self.device_id_facedetection)
self.facial_landmark_nn = face_alignment.FaceAlignment(
landmarks_type=face_alignment.LandmarksType._2D,
device=self.device_id_facealignment,
flip_input=False)
Server(ModelSizeConfig, self._dyn_reconfig_callback)
# have the subscriber the last thing that's run to avoid conflicts
self.color_sub = rospy.Subscriber("/image",
Image,
self.callback,
buff_size=2**24,
queue_size=1)
#!/usr/bin/env python
import rospy
from dynamic_reconfigure.server import Server
from tiburon.cfg import yawPitchDepthConfig
#Note: ypdConfig name is from gen.generate in the cfg file
def callback(config, level):
rospy.loginfo("ypd config successful!")
return config
if __name__ == "__main__":
rospy.init_node("ypd_reconfigure", anonymous=True)
srv = Server(yawPitchDepthConfig, callback)
rospy.spin()
#!/usr/bin/env python
import rospy
from dynamic_reconfigure.server import Server
from rowi.cfg import TestConfig
def callback(config, level):
# rospy.loginfo("""Reconfigure Request: {int_param}, {double_param},\
# {str_param}, {bool_param}, {size}""".format(**config))
print config, level
return config
if __name__ == "__main__":
rospy.init_node("dynamic_reconfigure_test", anonymous = True)
srv = Server(TestConfig, callback)
rospy.spin()
self.gesture_topic.publish(msg)
def get_emotion(self, node):
emotion = {}
emotion['name'] = node['animation'].strip(',')
emotion['start'] = node['start']
emotion['end'] = node['end']
return emotion
def sendEmotion(self, emotion):
msg = EmotionState()
args = emotion['name'].split(',', 2)
logger.info(args)
msg.magnitude = 1
msg.duration.secs = 1
if len(args) >= 1:
msg.name = str(args[0])
if len(args) >= 2:
msg.magnitude = float(args[1])
if len(args) >= 3:
msg.duration.secs = float(args[2])
logger.info("Send emotion {}".format(msg))
self.emotion_topic.publish(msg)
if __name__ == '__main__':
rospy.init_node('tts_talker')
talker = TTSTalker()
Server(TTSConfig, talker.reconfig)
rospy.spin()
loopRateHz = rospy.get_param('~loopHz', 20)
imageTopic = rospy.get_param('~image', '/bottomcam/camera/image_color')
depthTopic = rospy.get_param('~depth', '/depth')
compassTopic = rospy.get_param('~compass', '/euler')
expectedLanes = rospy.get_param('~lanes', 1)
outDir = rospy.get_param('~out', 'left')
TEST_MODE = rospy.get_param('~testmode', False)
test_heading = rospy.get_param('~heading', 0)
# Set up param configuration window
def configCallback(config, level):
for param in params:
params[param] = config[param]
return config
srv = Server(LaneMarkerDetectorConfig, configCallback)
camdebug = CamDebug('Vision', debugOn=DEBUG)
global inputHeading
inputHeading = 0 #TODO: take in the input heading for the previous task
global rosRate
rosRate = rospy.Rate(loopRateHz)
def gotRosFrame(rosImage):
if laneDetector: laneDetector.gotRosFrame(rosImage)
def gotHeading(msg):
if laneDetector: laneDetector.gotHeading(msg)
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',
10) # 160=purple, 100=blue, 10=Orange
self.color_range = rospy.get_param('~color_range', 15)
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)
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
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.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)
def callbackDynParam(config, level):
# -----------------------------------------------------------------------------
global kp
global ki
global kd
kp = float("""{kp}""".format(**config))
ki = float("""{ki}""".format(**config))
kd = float("""{kd}""".format(**config))
return config
# server for dyamic parameters
srv = Server(WheelPIDCFGConfig, callbackDynParam)
#init dynamic parameters
kp = rospy.get_param('/wheelPID/kp', 1.85)
ki = rospy.get_param('/wheelPID/ki', 2.25)
kd = rospy.get_param('/wheelPID/kd', 0.55)
# -----------------------------------------------------------------------------
def callBackLeftOmegaRef(data):
# -----------------------------------------------------------------------------
global robot
# assign desired wheel speed
robot.leftWheel.omegaRef = data.data
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
prevent_programming = rospy.get_param("/ur_driver/prevent_programming",
False)
reconfigure_srv = Server(URDriverConfig, reconfigure_callback)
## Still use parameter server?
#update = URDriverConfig(prevent_programming)
#reconfigure_srv.update_configuration(update)
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
gripper_service_provider = None
action_server = None
action_server_cart = None
try:
while not rospy.is_shutdown():
# Checks for disconnect
if getConnectedRobot(wait=False):
time.sleep(0.2)
prevent_programming = rospy.get_param(
"/ur_driver/prevent_programming", False)
if prevent_programming:
print "Programming now prevented"
connection.send_reset_program()
else:
print "Disconnected. Reconnecting"
if action_server:
action_server.set_robot(None)
if action_server_cart:
action_server_cart.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)
prevent_programming = rospy.get_param(
"/ur_driver/prevent_programming", False)
connection.send_program()
r = getConnectedRobot(
wait=True, timeout=3.0
) # increase timeout in case ur prog take too long to launch e.g. prog gets bigger
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 gripper_service_provider:
gripper_service_provider.set_robot(r)
else:
gripper_service_provider = GripperServiceProvider(r)
if action_server:
action_server.set_robot(r)
else:
action_server = URTrajectoryFollower(
r, rospy.Duration(1.0))
action_server.start()
if action_server_cart:
action_server_cart.set_robot(r)
else:
action_server_cart = URCartTrajectory(r)
action_server_cart.start()
except KeyboardInterrupt:
try:
r = getConnectedRobot(wait=False)
rospy.signal_shutdown("KeyboardInterrupt")
if r: r.send_quit()
except:
pass
raise
#!/usr/bin/env python
import rospy
from dynamic_reconfigure.server import Server
from opencog_control.cfg import OpenpsiConfig
def callback(config, level):
return config
if __name__ == "__main__":
rospy.init_node("openpsi_control")
srv = Server(OpenpsiConfig, callback)
rospy.spin()
def __init__(self):
#############################################################################
rospy.init_node("diff_tf")
self.nodename = rospy.get_name()
rospy.loginfo("-I- %s started" % self.nodename)
#### parameters #######
self.rate = rospy.get_param(
'~rate', 10.0) # the rate at which to publish the transform
self.ticks_meter = float(rospy.get_param(
'ticks_meter',
68500)) # The number of wheel encoder ticks per meter of travel
self.base_width = float(rospy.get_param(
'base_width', 0.65)) # The wheel base width in meters
self.base_frame_id = rospy.get_param(
'~base_frame_id',
'base_link') # the name of the base frame of the robot
self.odom_frame_id = rospy.get_param(
'~odom_frame_id',
'odom') # the name of the odometry reference frame
self.wheel_frame_id = rospy.get_param(
'~wheel_frame_id', 'chassis_link') # the name of wheel frame
self.encoder_min = rospy.get_param('encoder_min', -32768)
self.encoder_max = rospy.get_param('encoder_max', 32767)
self.encoder_low_wrap = rospy.get_param(
'wheel_low_wrap',
(self.encoder_max - self.encoder_min) * 0.3 + self.encoder_min)
self.encoder_high_wrap = rospy.get_param(
'wheel_high_wrap',
(self.encoder_max - self.encoder_min) * 0.7 + self.encoder_min)
# Odom covariances. I'm not going to be fusing these.
self.covar_x = rospy.get_param('covar_x', 1e6)
self.covar_y = rospy.get_param('covar_y', 1e6)
self.covar_z = rospy.get_param('covar_z', 1e6)
self.covar_roll = rospy.get_param('covar_roll', 1e6)
self.covar_pitch = rospy.get_param('covar_pitch', 1e6)
self.covar_yaw = rospy.get_param('covar_yaw', 0.3)
# These are the covariances that matter, xdot, ydot and yawdot.
self.covar_twist_x = rospy.get_param('covar_twist_x', 0.01)
self.covar_twist_y = rospy.get_param('covar_twist_y', 0.01)
self.covar_twist_z = rospy.get_param('covar_twist_z', 1e6)
self.covar_twist_roll = rospy.get_param('covar_twist_roll', 1e6)
self.covar_twist_pitch = rospy.get_param('covar_twist_pitch', 1e6)
self.covar_twist_yaw = rospy.get_param('covar_twist_yaw', 0.01)
self.publish_tf = rospy.get_param('~publish_tf', True)
rospy.loginfo("Parameter: publish_tf set to: {}".format(
self.publish_tf))
# Add a tf listener so we can transforms to the base link
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.t_delta = rospy.Duration(1.0 / self.rate)
self.t_next = rospy.Time.now() + self.t_delta
# internal data
self.enc_left = None # wheel encoder readings
self.enc_right = None
self.left = 0 # actual values coming back from robot
self.right = 0
self.lmult = 0
self.rmult = 0
self.prev_lencoder = 0
self.prev_rencoder = 0
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
self.dx = 0 # speeds in x/rotation
self.dr = 0
self.then = rospy.Time.now()
# The offset frame self.x2 = 0
self.y2 = 0
# subscriptions
rospy.Subscriber("lwheel", Int32, self.lwheelCallback, queue_size=1)
rospy.Subscriber("rwheel", Int32, self.rwheelCallback, queue_size=1)
self.odomPub = rospy.Publisher("odom", Odometry, queue_size=10)
self.odomBroadcaster = TransformBroadcaster()
self.updateParams()
srv = Server(DiffTfConfig, self.paramCallback)
def __init__(self):
# type: () -> None
"""
Initiate VisualCompassHandler
return: None
"""
# Init ROS package
rospack = rospkg.RosPack()
self.package_path = rospack.get_path('bitbots_visual_compass')
rospy.init_node('bitbots_visual_compass_setup')
rospy.loginfo('Initializing visual compass setup')
self.bridge = CvBridge()
self.config = {}
self.image_msg = None
self.compass = None
self.hostname = socket.gethostname()
# TODO: docs
self.base_frame = 'base_footprint'
self.camera_frame = 'camera_optical_frame'
self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(50))
self.listener = tf2.TransformListener(self.tf_buffer)
self.pub_head_mode = rospy.Publisher('head_mode',
HeadMode,
queue_size=1)
# Register VisualCompassConfig server for dynamic reconfigure and set callback
Server(VisualCompassConfig, self.dynamic_reconfigure_callback)
rospy.logwarn("------------------------------------------------")
rospy.logwarn("|| WARNING ||")
rospy.logwarn("||Please remove the LAN cable from the Robot, ||")
rospy.logwarn("||after pressing 'YES' you have 10 Seconds ||")
rospy.logwarn("||until the head moves OVER the LAN port!!! ||")
rospy.logwarn("------------------------------------------------\n\n")
try:
input = raw_input
except NameError:
pass
accept = input("Do you REALLY want to start? (YES/n)")
if accept == "YES":
rospy.logwarn("REMOVE THE LAN CABLE NOW!!!!!")
rospy.sleep(10)
head_mode = HeadMode()
head_mode.headMode = 10
self.pub_head_mode.publish(head_mode)
rospy.loginfo("Head mode has been set!")
rospy.spin()
else:
rospy.signal_shutdown(
"You aborted the process! Shuting down correctly.")
msg.command = self._effort
msg.set_point = self._setpoint
msg.process_value = self._position
msg.process_value_dot = self._velocity
msg.error = self._pid.error
msg.time_step = dt.to_sec()
msg.p = self._pid.p
msg.i = self._pid.i
msg.d = self._pid.d
# Publish to listeners
self._out_state.publish(msg)
if __name__ == '__main__':
# Create an anonymous ROS node
rospy.init_node('pid_controller', anonymous=True)
# Create the actual code that will interact with ROS
node = Node()
# Setup callback from Crustcrawler
rospy.Subscriber("/crustcrawler/joint_states", JointState,
node.joint_states_callback)
# Setup subscription to 'setpoint', this allows users to send messages
# to '/pid_controller/setpoint' to change our setpoint
rospy.Subscriber("~setpoint", Float64, node.setpoint_callback)
# Set up dynamic reconfigure so that we support online PID tuning
srv = Server(PidConfig, node.config_callback)
# Create timer so that our code is called at fixed rates
rospy.Timer(rospy.Duration(1. / 30.), node.update)
# Sleep until user or ROS quits
rospy.spin()
self.delay_response = config.delay_response
self.delay_time = config.delay_time
self.client.ignore_indicator = config.ignore_indicator
if config.set_that:
self.client.do_said(config.set_that)
config.set_that = ''
if config.set_context:
self.client.set_context(config.set_context)
self.client.set_marker(config.marker)
self.mute = config.mute
self.insert_behavior = config.insert_behavior
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()
rospy.loginfo('drivethru_srv initialized!')
dt = Drive_thru(False)
rospy.loginfo("Drive Thru loaded!")
# Set up param configuration window
def drivethruCallback(config, level):
global dt
dt.shape_hu = config['shape_hu']
for param in dt.orange_params:
dt.orange_params[param] = config['orange_' + param]
for param in drivethru_params:
drivethru_params[param] = config[param]
return config
srv = Server(DrivethruConfig, drivethruCallback)
# Service Client
if not isTest:
rospy.loginfo('waiting for mission_srv...')
rospy.wait_for_service('mission_srv')
mission_srv_request = rospy.ServiceProxy('mission_srv',
vision_to_mission,
headers={'id': '5'})
rospy.loginfo('connected to mission_srv!')
sm_top = smach.StateMachine(outcomes=['drivethru_complete', 'aborted'])
# Add overall States to State Machine for Gate Task
with sm_top:
smach.StateMachine.add('DISENGAGED',
Disengage(),
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:
print 'load locations'
self.object_dic = json.load(f)
for i in self.object_dic:
print i
# 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):
# 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', 0.5) # 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))
try:
self.tf_listener.waitForTransform(self.odom_frame, self.base_frame,
rospy.Time(),
rospy.Duration(60.0))
# self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(60.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
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 main(argv):
global node
node = ImgProcNode()
srv = Server(img_procConfig, dr_callback)
node.start()
return
