#!/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