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
