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('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 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 __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 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):
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("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 __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 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):
# 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):
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 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 baseMotNode():
'''
Subscriber Node for the base motor.
'''
init_node('baseMotor', anonymous=True)
Subscriber('motAngs', DOFArray, baseAngCall)
spin()
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, 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 __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 toolMotNode():
'''
Subscriber Node for the tool motor.
'''
init_node('toolMotor', anonymous=True)
Subscriber('motAngs', DOFArray, toolAngCall)
spin()
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 mainMotNode():
'''
Subscriber Node for the main motor.
'''
init_node('mainMotor', anonymous=True)
Subscriber('motAngs', DOFArray, mainAngCall)
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 secondaryMotNode():
'''
Subscriber Node for the second motor.
'''
init_node('secondaryMotor', anonymous=True)
Subscriber('motAngs', DOFArray, secAngCall)
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, 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):
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 = []
