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 main():
''' The main routine '''
init_node('dron_employee_ros')
# Create queues
route_queue = Queue()
position_queue = Queue()
arming_queue = Queue()
trgt_queue = Queue()
waypoints_queue = Queue()
# Create controller thread
controller = Thread(target=quad_controller,
args=(route_queue, position_queue, arming_queue,
trgt_queue, waypoints_queue))
# Route message handler
def quad_route(msg):
if not controller.isAlive():
controller.start()
route_queue.put(msg)
Subscriber('mavros/global_position/global', NavSatFix, position_queue.put)
Subscriber('armed', Bool, arming_queue.put)
# TODO: combine next two topic in one
Subscriber('target_request', NavSatFix, trgt_queue.put)
Subscriber('mavros/mission/waypoints', WaypointList, waypoints_queue.put)
Subscriber('route', RouteResponse, quad_route)
spin()
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):
self.pub_2 = Publisher('/leo/cmd_vel', Twist, queue_size=10)
self.sub_1 = Subscriber('/turtle1/pose', Pose, self.follow)
self.sub_2 = Subscriber('/leo/pose', Pose, self.update)
self.pose = Pose()
self.eps = 1
def main():
# Vrpn position subscriber
Subscriber("/vrpn_client_node/ardrone/pose", PoseStamped,
vrpn_handler, queue_size=1)
# Takeoff subscriber
Subscriber("/ardrone/vrpn_takeoff", Empty, takeoff_handler, queue_size=1)
# Landing subscriber
Subscriber("/ardrone/vrpn_land", Empty, land_handler, queue_size=1)
def arduino_interface():
global ecu_pub, motor_pwm, servo_pwm, ecu_cmd, shoulder, bicep
global pub1_shoulder,pub2_bicep,pub3_forearm,pub4_wrist,pub5_gripper
global shoulder_angle,bicep_angle,forearm_angle,wrist_angle,gripper_angle,initialized
global counter, tstart
init_node('arduino_interface')
tstart = time.time()
shoulder_angle = 0
bicep_angle = 0
forearm_angle = 0
wrist_angle = 0
gripper_angle = 0
counter=0
initialized = False
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
ecu_cmd = Twist()
time_prev = time.time()
#publishers
ecu_pub = rospy.Publisher('cmd_vel_mux/input/teleop',Twist, queue_size = 1)
pub1_shoulder = rospy.Publisher('arm_shoulder_pan_joint/command',Float64, queue_size = 1)
pub2_bicep = rospy.Publisher('arm_bicep_joint/command',Float64, queue_size = 1)
pub3_forearm = rospy.Publisher('arm_forearm_joint/command',Float64, queue_size = 1)
pub4_wrist = rospy.Publisher('arm_wrist_flex_joint/command',Float64, queue_size = 1)
pub5_gripper = rospy.Publisher('gripper_joint/command',Float64, queue_size = 1)
#subscribers
imu_sub1 =Subscriber('imu/data1', Imu, callback_function1)
imu_sub2 =Subscriber('imu/data2', Imu, callback_function2)
jointState_sub = rospy.Subscriber('/joint_states', JointState, callback_function)
tutorial = MoveGroupPythonIntefaceTutorial()
while not rospy.is_shutdown():
if counter==0 and initialized:
raw_input()
tutorial.go_to_joint_state([0,0,0,0,0])
counter+=1
# wait
rate.sleep()
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 main():
Subscriber("/ardrone/true_position",
PoseStamped,
true_pos_handler,
queue_size=1)
Subscriber("/vrpn_client_node/paddle_1/pose",
PoseStamped,
paddle_1_handler,
queue_size=1)
Subscriber("/vrpn_client_node/paddle_2/pose",
PoseStamped,
paddle_2_handler,
queue_size=1)
def __init__(self):
init_node("omni_feedback_force", anonymous=False)
self._getParameters()
self._publisher = Publisher(self._outputTopic,
OmniFeedback,
queue_size=100)
self._stiffnessSub = Subscriber(self._stiffnessInput,
Float32MultiArray,
self._stiffnessCallback)
self._positionSub = Subscriber(self._omniPositionInput, LockState,
self._omniPositionCallback)
self._stiffnessMessage = []
self._omniPositionMessage = []
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):
self._lidar_topic = Subscriber(LASER_SCAN_TOPIC, LaserScan, self.on_lidar_update, queue_size=1)
self._odometry_topic = Subscriber(ODOMETRY_TOPIC, Odometry, self.on_odometry_update, queue_size=1)
self._drive_topic = Publisher(DRIVE_TOPIC, AckermannDriveStamped, queue_size=1)
self._lidar_data: Optional[LaserScan] = None
self._odometry_data: Optional[Odometry] = None
self._command: AckermannDriveStamped = None
self._velocity: float = 0
self._steering_angle: float = 0
# Need to record lap-related information for measuring approximated lap time
self._lap_start_timestamp: Optional[float] = None
self._race_start_position: Optional[Tuple[CartesianPoint, CartesianPoint]] = None
self._check_lap_finished: bool = False
def __init__(self):
self.Tc = CompactTransform.fromXYZRPY(0.0, 0.0, 0.0, 0.0, 0.0,
-20 * RAD2DEG)
self.got_pose = False
self.got_reference = False
self.velocity_pub = Publisher(
'/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=1)
joy_sub = Subscriber('/joy', Joy, self.joy_callback)
pose_sub = Subscriber('/mavros/vision_pose/pose', PoseStamped,
self.pose_callback)
reference_sub = Subscriber('/mavros/control/reference', PoseStamped,
self.reference_callback)
def arduino_interface():
# launch node, subscribe to motorPWM and servoPWM, publish ecu
init_node('arduino_interface')
llc = low_level_control()
Subscriber('ecu0', ECU, v0_callback, queue_size = 1)
Subscriber('ecu2', ECU, llc.pwm_converter_callback, queue_size = 1)
# Subscriber('pos_info', pos_info, SE_callback, queue_size = 1)
llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 1)
# Set motor to neutral on shutdown
on_shutdown(llc.neutralize)
# process callbacks and keep alive
spin()
def __init__(self, name, topic, action_type):
""" Creating a custom mock action server."""
self._topic = topic
self._name = name
self._action_type = action_type
self.timeout = 5
self.action_result = None
Subscriber('mock/' + name, String, self.receive_commands)
Subscriber('mock/gui_result', Bool, self.set_gui_result)
self._server = ActionServer(self._topic, self._action_type,
self.success, False)
self._server.start()
loginfo('>>> Starting ' + self._name)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
eprint('Building Listener.')
init_node('listener', anonymous=True)
Subscriber('sonar_meas', Float64WithHeader, sonar_callback)
Subscriber('imu_data', Imu, imu_callback)
Subscriber('yaw_rate', Float64WithHeader, yaw_callback)
eprint('Spinning.')
# spin() simply keeps python from exiting until this node is stopped
spin()
def __init__(self):
# Read Parameters From YAML File
self.turtlebot_odom_topic = get_param('turtlebot_odom_topic')
self.turtlebot_base_footprint_frame = get_param(
'turtlebot_base_footprint_frame')
self.map_frame = get_param('map_frame')
self.drone_odom_frame = get_param('drone_odom_frame')
self.join_tree_srv_name = get_param('join_tree_srv')
self.debug = get_param('debug')
# TF Broadcaster
self.tf_broadcast = TransformBroadcaster()
# TF Listener
self.tf_listen = Buffer()
self.tf_listener = TransformListener(self.tf_listen)
# Subscribers
Subscriber(self.turtlebot_odom_topic, Odometry, self.turtlebot_odom_cb)
# Services for Joining the Drone TF Tree to Main Tree (Localization)
self.join_tree_service = Service(self.join_tree_srv_name, JointTrees,
self.join_tree_srv_function)
# Parameters
self.drone_tf = TransformStamped()
self.turtlebot_tf = TransformStamped()
self.turtlebot_tf.header.frame_id = self.map_frame
self.turtlebot_tf.child_frame_id = self.turtlebot_base_footprint_frame
self.drone_tf.header.frame_id = self.map_frame
self.drone_tf.child_frame_id = self.drone_odom_frame
self.drone_tf.transform.rotation.w = 1.0
self.turtlebot_tf.transform.rotation.w = 1.0
# Flags
self.join_trees = False # Set True When The Two TF Trees are Joint in One
def __init__(self, topic, data_class): # type: (str, T) -> None
self._topic = topic
self._subscriber = Subscriber(topic,
data_class,
self._callback,
queue_size=1)
self._value = None # type: T
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 main():
Subscriber("/ardrone/true_position", PoseStamped, true_pos_handler,
queue_size=1)
while True:
target = Pose()
target.position.x = 1.7
target.position.y = -0.3
target.position.z = 1
target_pub.publish(target)
sleep(5)
target = Pose()
target.position.x = 1.7
target.position.y = -1.3
target.position.z = 1
target_pub.publish(target)
sleep(5)
target = Pose()
target.position.x = 0.3
target.position.y = -1.3
target.position.z = 1
target_pub.publish(target)
sleep(5)
target = Pose()
target.position.x = 0.3
target.position.y = -0.3
target.position.z = 1
target_pub.publish(target)
sleep(5)
def baseMotNode():
'''
Subscriber Node for the base motor.
'''
init_node('baseMotor', anonymous=True)
Subscriber('motAngs', DOFArray, baseAngCall)
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 __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 mainMotNode():
'''
Subscriber Node for the main motor.
'''
init_node('mainMotor', anonymous=True)
Subscriber('motAngs', DOFArray, mainAngCall)
spin()
def secondaryMotNode():
'''
Subscriber Node for the second motor.
'''
init_node('secondaryMotor', anonymous=True)
Subscriber('motAngs', DOFArray, secAngCall)
spin()
def __init__(self, client, topic_name, ros_type):
self._topic_name = topic_name
self._ros_type_name = ros_type
self.__ros_type = self.__import_type()
self.__client = client
self.__topic_subscriber = Subscriber(self._topic_name, self.__ros_type,
self.__callback)
def toolMotNode():
'''
Subscriber Node for the tool motor.
'''
init_node('toolMotor', anonymous=True)
Subscriber('motAngs', DOFArray, toolAngCall)
spin()
def __init__(self):
self.tf_broadcaster = TransformBroadcaster()
self.ts = TransformStamped()
self.ts.header.frame_id = 'world'
self.ts.child_frame_id = 'drone'
pose_sub = Subscriber('/dji_sdk/pose', PoseStamped, self.pose_callback)
def __init__(self, environment):
self.robot_position_sub = Subscriber("/gazebo/model_states",
ModelStates,
self.on_position_update)
self.current_pos = np.zeros(shape=3)
self.last_poses = deque(3 * [np.zeros(3)], maxlen=5)
self.start_time = time.time()
super(RLTask, self).__init__(environment)
def __init__(self):
init_node("draw_ellipsoid", anonymous=False)
self._publisher = Publisher("/ellipsoid_visualization",
Marker,
queue_size=50)
self._subscribers = Subscriber("/covariance_matrix", Float32MultiArray,
self._callback)
self._getParameters()
self._multiArray = []
def __init__(self, params, servoNum, output):
Device.__init__(self, params.getServoName(servoNum), output)
self._servoNum = servoNum
self._pub = Publisher('%s/Position' % self._name,
Float32,
queue_size=params.getServoPublishHz(servoNum))
Subscriber('%s/command' % self._name, Float32, self.servoCallBack)
#KeepAliveHandler('%s/Position' % self._name, Float32)
self._haveRightToPublish = False
def __init__(self, topic, msg_type=None):
""" Register a subscriber on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the subscriber on
msg_type -- (optional) the type to register the subscriber as. If not
provided, an attempt will be made to infer the topic type
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topic_type = get_topic_type(topic)[0]
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
if topic_type is not None and topic_type != msg_class._type:
raise TypeConflictException(topic, topic_type, msg_class._type)
# Create the subscriber and associated member variables
self.subscriptions = {}
self.lock = Lock()
self.topic = topic
self.msg_class = msg_class
self.subscriber = Subscriber(topic, msg_class, self.callback)
class MultiSubscriber():
""" Handles multiple clients for a single subscriber.
Converts msgs to JSON before handing them to callbacks. Due to subscriber
callbacks being called in separate threads, must lock whenever modifying
or accessing the subscribed clients. """
def __init__(self, topic, msg_type=None):
""" Register a subscriber on the specified topic.
Keyword arguments:
topic -- the name of the topic to register the subscriber on
msg_type -- (optional) the type to register the subscriber as. If not
provided, an attempt will be made to infer the topic type
Throws:
TopicNotEstablishedException -- if no msg_type was specified by the
caller and the topic is not yet established, so a topic type cannot
be inferred
TypeConflictException -- if the msg_type was specified by the
caller and the topic is established, and the established type is
different to the user-specified msg_type
"""
# First check to see if the topic is already established
topic_type = get_topic_type(topic)[0]
# If it's not established and no type was specified, exception
if msg_type is None and topic_type is None:
raise TopicNotEstablishedException(topic)
# Use the established topic type if none was specified
if msg_type is None:
msg_type = topic_type
# Load the message class, propagating any exceptions from bad msg types
msg_class = ros_loader.get_message_class(msg_type)
# Make sure the specified msg type and established msg type are same
if topic_type is not None and topic_type != msg_class._type:
raise TypeConflictException(topic, topic_type, msg_class._type)
# Create the subscriber and associated member variables
self.subscriptions = {}
self.lock = Lock()
self.topic = topic
self.msg_class = msg_class
self.subscriber = Subscriber(topic, msg_class, self.callback)
def unregister(self):
self.subscriber.unregister()
with self.lock:
self.subscriptions.clear()
def verify_type(self, msg_type):
""" Verify that the subscriber subscribes to messages of this type.
Keyword arguments:
msg_type -- the type to check this subscriber against
Throws:
Exception -- if ros_loader cannot load the specified msg type
TypeConflictException -- if the msg_type is different than the type of
this publisher
"""
if not ros_loader.get_message_class(msg_type) is self.msg_class:
raise TypeConflictException(self.topic,
self.msg_class._type, msg_type)
return
def subscribe(self, client_id, callback):
""" Subscribe the specified client to this subscriber.
Keyword arguments:
client_id -- the ID of the client subscribing
callback -- this client's callback, that will be called for incoming
messages
"""
with self.lock:
self.subscriptions[client_id] = callback
# If the topic is latched, add_callback will immediately invoke
# the given callback.
self.subscriber.impl.add_callback(self.callback, [callback])
self.subscriber.impl.remove_callback(self.callback, [callback])
def unsubscribe(self, client_id):
""" Unsubscribe the specified client from this subscriber
Keyword arguments:
client_id -- the ID of the client to unsubscribe
"""
with self.lock:
del self.subscriptions[client_id]
def has_subscribers(self):
""" Return true if there are subscribers """
with self.lock:
ret = len(self.subscriptions) != 0
return ret
def callback(self, msg, callbacks=None):
""" Callback for incoming messages on the rospy.Subscriber
Converts the incoming msg to JSON, then passes the JSON to the
registered subscriber callbacks.
Keyword Arguments:
msg - the ROS message coming from the subscriber
callbacks - subscriber callbacks to invoke
"""
# Try to convert the msg to JSON
json = None
try:
json = message_conversion.extract_values(msg)
except Exception as exc:
logerr("Exception while converting messages in subscriber callback : %s", exc)
return
# Get the callbacks to call
if not callbacks:
with self.lock:
callbacks = self.subscriptions.values()
# Pass the JSON to each of the callbacks
for callback in callbacks:
try:
callback(json)
except Exception as exc:
# Do nothing if one particular callback fails except log it
logerr("Exception calling subscribe callback: %s", exc)
pass
