def __init__(self): init_node("qr_code_manager", anonymous=True) self.markers = dict() self.init_markers() self.pub = Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=5) self.stamp_string_pub = Publisher( "/visp_auto_tracker/code_message_stamped", StringStamped, queue_size=5) self.stamp_string_sub = Subscriber("/visp_auto_tracker/code_message", String, self.stamp_string_callback) self.stamp_position_sub = Subscriber( "/visp_auto_tracker/object_position_covariance", PoseWithCovarianceStamped, self.stamp_position_callback) self.code_stamped = StringStamped() self.position_stamped = PoseWithCovarianceStamped() self.baseLinkFrameId = rospy.get_param('~base_link_frame_id', '/base_link') self.mapFrameId = rospy.get_param('~map_frame_id', '/map') self.tr = tf.TransformerROS() self.tl = tf.TransformListener() self.d = {}
def __init__(self, type): smach.State.__init__(self, outcomes=['done', 'failed', 'preempted'], input_keys=[ 'robot_id', 'velocity', 'steering', 'token_id', 'enable' ]) if (type == 'CAM'): self._pub = Publisher('networking/cam', Cam, queue_size=10) self.type = type elif (type == 'TOKEN'): self._pub = Publisher('/networking/token', Token, queue_size=10) self.type = type elif (type == 'EMERGENCY_BRAKE'): self._pub = Publisher('/networking/emergency_brake', EmergencyBrake, queue_size=10) self.type = type elif (type == 'PLATOONING'): self._pub = Publisher('/networking/platooning', Platooning, queue_size=10) self.type = type elif (type == 'COMMAND'): self._pub = Publisher('/networking/command', Command, queue_size=10) self.type = type else: rospy.logerr('Unknown message type')
def setUp(self): self.agent = Agent(strategy='normal') self.agent.machine.add_state('test_validate_gui') self.agent.machine.add_transition('operator_responded', 'off', 'test_validate_gui') self.validate_gui_mock = Publisher('mock/validate_gui', String) self.gui_result = Publisher('mock/gui_result', Bool) sleep(2)
def __init__(self): init_node("mock_omni_position", anonymous=True) self._getParameters() self._cycle_i = 1 # Initialize as 1 for first cycle self._startTime = get_rostime() # get start time ros self._passedTime = 0 # initialized passed time self._pubPosition= Publisher(self._outputTopicPosition, LockState , queue_size=1) self._pubButton = Publisher(self._outputTopicButton, PhantomButtonEvent , queue_size=1)
class MarkerPublisherChannel(Enum): """ Available publisher channels for marker messages. """ FIRST = Publisher("/visualisation_marker_01", Marker, queue_size=100) SECOND = Publisher("/visualisation_marker_02", Marker, queue_size=100) THIRD = Publisher("/visualisation_marker_03", Marker, queue_size=100) FOURTH = Publisher("/visualisation_marker_04", Marker, queue_size=100) FIFTH = Publisher("/visualisation_marker_05", Marker, queue_size=100)
def setUp(self): rospy.init_node('test_move_end_effector_server_node') self.effector_client = Client(move_end_effector_controller_topic, MoveEndEffectorAction) self.server = MoveEndEffectorServer() self.goal = MoveEndEffectorGoal() self.goal_maker = EffectorGoalMaker() self.sensor_pub = Publisher('mock/sensor', String) self.linear_actuator_pub = Publisher('mock/linear_actuator', String) self.head_pub = Publisher('mock/head', String) rospy.sleep(1)
def __init__(self): self.authority = False control_authority_srv = Service('/dji_sdk/sdk_control_authority', SDKControlAuthority, self.handler) self.control_pub = Publisher('/dji_sdk/matlab_rpvy', Joy, queue_size=1) self.wind_pub = Publisher('/dji_sdk/matlab_wind', Joy, queue_size=1) joy_sub = Subscriber('/dji_sdk/xbox_joy', Joy, self.joy_callback) control_sub = Subscriber( '/dji_sdk/flight_control_setpoint_rollpitch_yawrate_zvelocity', Joy, self.control_callback)
def __init__(self): init_node('iiwa_sunrise', log_level = INFO) hardware_interface = get_param('~hardware_interface', 'PositionJointInterface') self.robot_name = get_param('~robot_name', 'iiwa') model = get_param('~model', 'iiwa14') tool_length = get_param('~tool_length', 0.0) if model == 'iiwa7': self.l02 = 0.34 self.l24 = 0.4 elif model == 'iiwa14': self.l02 = 0.36 self.l24 = 0.42 else: logerr('unknown robot model') return self.l46 = 0.4 self.l6E = 0.126 + tool_length self.tr = 0.0 self.v = 1.0 self.joint_names = ['{}_joint_1'.format(self.robot_name), '{}_joint_2'.format(self.robot_name), '{}_joint_3'.format(self.robot_name), '{}_joint_4'.format(self.robot_name), '{}_joint_5'.format(self.robot_name), '{}_joint_6'.format(self.robot_name), '{}_joint_7'.format(self.robot_name)] joint_states_sub = Subscriber('joint_states', JointState, self.jointStatesCb, queue_size = 1) command_pose_sub = Subscriber('command/CartesianPose', PoseStamped, self.commandPoseCb, queue_size = 1) command_pose_lin_sub = Subscriber('command/CartesianPoseLin', PoseStamped, self.commandPoseLinCb, queue_size = 1) redundancy_sub = Subscriber('command/redundancy', Float64, self.redundancyCb, queue_size = 1) joint_position_sub = Subscriber('command/JointPosition', JointPosition, self.jointPositionCb, queue_size = 1) self.state_pose_pub = Publisher('state/CartesianPose', PoseStamped, queue_size = 1) self.joint_trajectory_pub = Publisher( '{}_trajectory_controller/command'.format(hardware_interface), JointTrajectory, queue_size = 1) path_parameters_configuration_srv = Service( 'configuration/setSmartServoLimits', SetSmartServoJointSpeedLimits, self.handlePathParametersConfiguration) path_parameters_lin_configuration_srv = Service( 'configuration/setSmartServoLinLimits', SetSmartServoLinSpeedLimits, self.handlePathParametersLinConfiguration) smart_servo_configuration_srv = Service( 'configuration/ConfigureControlMode', ConfigureControlMode, self.handleSmartServoConfiguration) spin()
def __init__(self): super(ProportionalDriveState, self).__init__(outcomes=['ok'], input_keys=['target_position']) self._twist_topic = '/drive_controller/cmd_vel' self._odom_topic = '/ekf/odom/odometry/filtered' self._visual_topic = '/viz/proportional_drive/target_position' self._twist_publisher = Publisher(self._twist_topic, Twist, queue_size=1) self._odometry = SubscriberValue(self._odom_topic, Odometry) self._visual_publisher = Publisher(self._visual_topic, PointStamped, queue_size=1) self._target_position = None # type: PointStamped self.STOPPING_DISTANCE = 0.1 self.FORWARD_SPEED = 1.0 self.MAX_ANGULAR_SPEED = 6.28 self.PROPORTIONAL_CONSTANT = 6.0
def __init__(self): rospy.init_node("motors_driver_sim") rospy.sleep(0.5) self.wheel_speeds_sub = Subscriber("wheel_speeds_cmd", WheelSpeeds, self.on_wheel_speeds) self.l_wheel_cmd_pub = Publisher("left_wheel_driver/command", Float64, queue_size=10) self.r_wheel_cmd_pub = Publisher("right_wheel_driver/command", Float64, queue_size=10) rospy.spin()
def __init__(self): super().__init__() self.set_node_status("serial_interface", "interface", NodeStatus.INIT) # Parse devices self.devices: DeviceList = DeviceList.parse_file( "can/devices.xml", package="interface_description") # Then frames self.frames: Dict[str, Frame] = FrameList.parse_file( "can/frames.xml", package="interface_description", context=Context(devices=self.devices)) # Create serial bus bus with given interface self.serials = [ SerialBus("/dev/ttyUSB0", self), SerialBus("/dev/ttyUSB1", self) ] self.subscriber: Subscriber = Subscriber("/can_interface/out", CanData, self.on_ros_message) self.publisher: Publisher = Publisher("/can_interface/in", CanData, queue_size=10) # Declare can_interface ready self.set_node_status("serial_interface", "interface", NodeStatus.READY)
def main_auto(): # initialize ROS node init_node('auto_mode', anonymous=True) nh = Publisher('ecu', ECU, queue_size=10) # set node rate rateHz = get_param("controller/rate") rate = Rate(rateHz) dt = 1.0 / rateHz t_i = 0 # get experiment parameters t_0 = get_param("controller/t_0") # time to start test t_f = get_param("controller/t_f") # time to end test FxR_target = get_param("controller/FxR_target") d_f_target = pi / 180 * get_param("controller/d_f_target") # main loop while not is_shutdown(): # get command signal (FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target) # send command signal ecu_cmd = ECU(FxR, d_f) nh.publish(ecu_cmd) # wait t_i += dt rate.sleep()
def arduino_interface(): global ecu_pub, motor_pwm, servo_pwm # initialize node init_node('arduino_interface') # setup publishers and subscribers loop_rate = 50 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) time_prev = time.time() ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10) v_meas_pub = rospy.Publisher('v_meas', Float32, queue_size=10) rospy.Subscriber('encoder', Encoder, enc_callback) while not rospy.is_shutdown(): if time.time() >= time_prev and time.time() < time_prev + 4: motor_pwm = 1650 if time.time() >= time_prev + 4: motor_pwm = 1500 ecu_cmd = ECU(motor_pwm, servo_pwm) ecu_pub.publish(ecu_cmd) v_meas_pub.publish(Float32(v_meas)) # wait rate.sleep()
def __init__(self): super().__init__() self.set_node_status("can_interface", "interface", NodeStatus.INIT) # Parse devices self.devices: DeviceList = DeviceList.parse_file( "can/devices.xml", package="interface_description" ) # Then frames self.frames: Dict[str, Frame] = FrameList.parse_file( "can/frames.xml", package="interface_description", context=Context(devices=self.devices) ) # Create can bus with given interface self.bus = can.interface.Bus(rospy.get_param("/interface/can_interface/device", default="can0")) self.can_input_thread = threading.Thread(name="can_input", target=self.wait_for_can_message) # TODO /can_interface/out as a service self.subscriber: Subscriber = Subscriber("/can_interface/out", CanData, self.on_ros_message) self.publisher: Publisher = Publisher("/can_interface/in", CanData, queue_size=100) # Start thread self.can_input_thread.start() # Declare can_interface ready self.set_node_status("can_interface", "interface", NodeStatus.READY)
def __init__(self, name, frame_id, min_angle, max_angle, angle_increment, min_range, max_range, scan_time=None, time_increment=None, queue_size=10): # Create the publisher self._publisher = Publisher(name, LaserScan, queue_size=queue_size) # Create a the laser scan message. Most of the message is static, the dynamic # fields are updated in _msg(). self._scan_msg = LaserScan() self._scan_msg.header.frame_id = frame_id self._scan_msg.angle_max = max_angle self._scan_msg.angle_min = min_angle self._scan_msg.angle_increment = angle_increment self._scan_msg.range_min = float(min_range) self._scan_msg.range_max = float(max_range) # Note: scan_time is the time between scans of the laser, i.e., the time it takes to read 360 degrees. self._scan_msg.scan_time = scan_time # Note: time_increment is the time between measurements, i.e. how often we read the scan and publish it (in # seconds) self._scan_msg.time_increment = time_increment # Per ROS documentation, the intensities filed should remain empty unless utilized by the sensor self._scan_msg.intensities = [] # Storage of the last time for calculating the scan time self._last_scan_time = Time.now()
def setUp(self): rospy.init_node('state_fusion_validation_test') self.agent = Agent(strategy='normal') self.fusion_validate = Publisher('mock/fusion_validate', String) self.target = mock_msgs.create_victim_info(id=1, probability=0.65) self.agent.target.set(self.target) self.agent.set_breakpoint('exploration')
def __init__(self, params=(0.9, 0.0008, 0.0008), angle_speeds=None): if angle_speeds is None: angle_speeds = {20: 35, 10: 55, 0: 75} lidar_topic = '/scan' drive_topic = '/drive' self.lidar_sub = Subscriber(lidar_topic, LaserScan, self.lidar_callback) self.drive_pub = Publisher(drive_topic, AckermannDriveStamped, queue_size=5) self.d = 0.0 self.window_size = 1 self.theta = math.pi / 4 self.kp, self.ki, self.kd = params self.prev_error = 0.0 self.error = 0.0 self.square_error = 0.0 self.integral = 0.0 self.min_angle = 45 self.max_angle = 135 self.servo_offset = 90.0 self.angle = 90.0 self.velocity = 1.0 self.freq = 300 self.drive_msg = AckermannDriveStamped() self.drive_msg.header.stamp = Time.now() self.drive_msg.header.frame_id = 'laser' self.drive_msg.drive.steering_angle = 90 self.drive_msg.drive.speed = 0.0 self.speed_per_angle = angle_speeds
def publish_message(self): import rospy from rospy import Publisher from geometry_msgs.msg import Twist rospy.init_node('move') rospy.loginfo("About to be moving!") publisher = Publisher('mobile_base/commands/velocity', Twist) twist = Twist() while True: rospy.loginfo("Current/Desired/Step Linear Speed: " + str(twist.linear.x) + "/" + str(self.linear.value) + "/" + str(self.LINEAR_STEP_SPEED)) rospy.loginfo("Current/Desired/Step Angular Speed: " + str(twist.angular.z) + "/" + str(self.angular.value) + "/" + str(self.ANGULAR_STEP_SPEED)) twist.linear.x = Driver.acceleration( current_speed=twist.linear.x, desired_speed=self.linear.value, step_speed=self.LINEAR_STEP_SPEED) twist.angular.z = Driver.acceleration( current_speed=twist.angular.z, desired_speed=self.angular.value, step_speed=self.ANGULAR_STEP_SPEED) publisher.publish(twist) rospy.sleep(0.1)
def __init__(self): self._cameraInfoPublisher = Publisher( "/xtion/rgb/camera_info_improved", CameraInfoMessage, queue_size=10) init_node("mock_camera", anonymous=True)
def __init__(self): # TF Listener self.tf = Buffer() self.tf_listener = TransformListener(self.tf) # Flags self.initialize = False # Parameters self.last_t = 0.0 self.lin_i_x = 0.0 self.lin_i_y = 0.0 self.kp = 0.0 self.ki = 0.0 self.kd = 0.0 self.error = 0.15 self.max_speed = 1.8 self.velocity = Twist() self.empty = Twist() # FIXME: Parameter from YAML File self.drone_base_link = get_param('drone_base_link') self.map_frame = get_param('map_frame') self.drone_speed_topic = get_param('drone_speed_topic') # Publish Speed Topic self.cmd_topic = Publisher(self.drone_speed_topic, Twist, queue_size=10)
def __init__(self, config): super().__init__() node_name = config.get('node_name', 'GazeboEnvNode') rospy.init_node(node_name, disable_signals=True) self.gz_state = GazeboState.INIT self.config = config self.state_pub = Publisher('/gazebo/set_model_state', ModelState, queue_size=10) self.pause = ServiceProxy("/gazebo/pause_physics", Empty) self.unpause = ServiceProxy("/gazebo/unpause_physics", Empty) self.reset_proxy = ServiceProxy("/gazebo/reset_simulation", Empty) self.add_agents() time.sleep(0.2) self.action_space = Dict( {k: v.action_space for k, v in self.agents.items()}) self.observation_space = Dict( {k: v.observation_space for k, v, in self.agents.items()}) self.unpause_gazebo() # check_for_ROS() self.pause_gazebo() print('=' * 100) print(self) print(rospy.get_name()) print(rospy.get_namespace()) print(rospy.get_node_uri()) print('=' * 100)
def arduino_interface(): global ecu_pub, motor_pwm, servo_pwm init_node('arduino_interface') # set node rate loop_rate = 50 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) time_prev = time.time() ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 10) motor_pwm = 1600 servo_pwm = 1400 flag = 0 while not rospy.is_shutdown(): # if time.time()-time_prev>=3: # servo_pwm = 1540 if time.time()-time_prev >= 8: motor_pwm = 1500 ecu_cmd = ECU(motor_pwm, servo_pwm) ecu_pub.publish(ecu_cmd) break ecu_cmd = ECU(motor_pwm, servo_pwm) ecu_pub.publish(ecu_cmd) # wait rate.sleep()
def __init__(self, package=None, mesh_folder_path=None): # interface to the planning and collision scenes self.psi = PlanningSceneInterface() # self.scene_pub = Publisher("/collision_object", CollisionObject, # queue_size=1, latch=True) self.vis_pub = Publisher("/collision_scene", MarkerArray, queue_size=10) self.marker_array = MarkerArray() sleep(1.0) if package is not None and mesh_folder_path is not None: # Build folder path for use in load rospack = RosPack() self.package = package self.mesh_folder_path = mesh_folder_path self.package_path = rospack.get_path(package) self.folder_path = os.path.join(self.package_path, mesh_folder_path)\ + os.sep else: loginfo( "Missing package or mesh folder path supplied to planning_scene_helper; no meshes can be added" ) # Create dict of objects, for removing markers later self.objects = {} self.next_id = 1
def __init__(self, linear_vel: float, angular_vel: float): super().__init__(outcomes=['ok', 'preempted']) self.cmd_vel_pub = Publisher(CMD_VEL_TOPIC, Twist, queue_size=1) self.rate = Rate(10) self.duration = Duration(1) self.linear_vel = linear_vel self.angular_vel = angular_vel
def main(): ''' The main routine ''' init_node('de_employee_ros') # Create queues target_queue = Queue() position_queue = Queue() arming_queue = Queue() # Create homebase publisher homebase_pub = Publisher('/de_employee/homebase', SatPosition, queue_size=1) # Create controller thread controller = Thread(target=quad_controller, args=(target_queue, position_queue, arming_queue, homebase_pub)) # Target message handler def quad_target(msg): if not controller.isAlive(): controller.start() target_queue.put(msg) else: print('Target already exist!') Subscriber('/mavros/global_position/global', NavSatFix, position_queue.put) Subscriber('/de_employee/armed', Bool, arming_queue.put) Subscriber('/de_employee/target', SatPosition, quad_target) spin()
def __init__(self, name, message, publisher_queue=100): self.message = message self.name = resolve_name(name) self.publisher = Publisher(self.name, self.message, queue_size=publisher_queue) self.subscribers = {}
def arduino_interface(): global ecu_pub, motor_pwm, servo_pwm init_node('arduino_interface') # set node rate loop_rate = 50 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) time_prev = time.time() ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10) while not rospy.is_shutdown(): if time.time() >= time_prev and time.time() < time_prev + 6.5: motor_pwm = 1650 if time.time() >= time_prev + 6.5: motor_pwm = 1500 #1465 #if time.time() >= time_prev + 5 and time.time() < time_prev + 10: # motor_pwm = 84.0 #if time.time() >= time_prev + 12 and time.time() < time_prev + 17: # motor_pwm = 86.0 ecu_cmd = ECU(motor_pwm, servo_pwm) ecu_pub.publish(ecu_cmd) break ecu_cmd = ECU(motor_pwm, servo_pwm) ecu_pub.publish(ecu_cmd) # wait rate.sleep()
def arduino_interface(): global ecu_pub, motor_pwm, servo_pwm init_node('arduino_interface') # set node rate loop_rate = 50 dt = 1.0 / loop_rate rate = rospy.Rate(loop_rate) time_prev = time.time() ecu_pub = Publisher('ecu_pwm', ECU, queue_size=10) while not rospy.is_shutdown(): if time.time() >= time_prev and time.time() < time_prev + 4: motor_pwm = 1580.0 if time.time() >= time_prev + 4 and time.time() < time_prev + 6: motor_pwm = 1620.0 if time.time() >= time_prev + 6 and time.time() < time_prev + 8: motor_pwm = 1600.0 if time.time() >= time_prev + 8 and time.time() < time_prev + 10: motor_pwm = 1500.0 if time.time() >= time_prev + 10: break ecu_cmd = ECU(motor_pwm, servo_pwm) ecu_pub.publish(ecu_cmd) # wait rate.sleep()
def initROS(self): init_node("IR_IRUS_sensors") on_shutdown(self.stop) nb_pub = 0 for sensor in self.sensors.values(): if sensor.FRAME_ID != None: nb_pub += len(sensor.FRAME_ID) self.publisher = Publisher("ir_irus", Range, queue_size=nb_pub) self.msg = Range()
def setUp(self): # Register the mock servers. rospy.init_node('unit_base_control') self.move_base_mock = Publisher('mock/feedback_move_base', String) self.agent = Agent(strategy='normal') target = mock_msgs.create_victim_info(id=8, probability=0.65) self.agent.target_victim = target