def replier(service_name, number_of_cycles, namespace):
from test_msgs.service_fixtures import get_test_srv
import rclpy
service_pkg = 'test_msgs'
module = importlib.import_module(service_pkg + '.srv')
srv_mod = getattr(module, service_name)
rclpy.init(args=[])
node = rclpy.create_node('replier', namespace=namespace)
srv_fixtures = get_test_srv(service_name)
chatter_callback = functools.partial(
replier_callback, srv_fixtures=srv_fixtures)
node.create_service(
srv_mod, 'test/service/' + service_name, chatter_callback)
spin_count = 1
print('replier: beginning loop')
while rclpy.ok() and spin_count < number_of_cycles:
rclpy.spin_once(node, timeout_sec=2)
spin_count += 1
print('spin_count: ' + str(spin_count))
node.destroy_node()
rclpy.shutdown()
def main(args=None):
global logger
rclpy.init(args=args)
node = rclpy.create_node('minimal_action_server')
logger = node.get_logger()
# Use a ReentrantCallbackGroup to enable processing multiple goals concurrently
# Default goal callback accepts all goals
# Default cancel callback rejects cancel requests
action_server = ActionServer(
node,
Fibonacci,
'fibonacci',
execute_callback=execute_callback,
cancel_callback=cancel_callback,
callback_group=ReentrantCallbackGroup())
# Use a MultiThreadedExecutor to enable processing goals concurrently
executor = MultiThreadedExecutor()
rclpy.spin(node, executor=executor)
action_server.destroy()
node.destroy_node()
rclpy.shutdown()
def func_destroy_timers():
import rclpy
rclpy.init()
node = rclpy.create_node('test_node3')
timer1 = node.create_timer(0.1, None)
timer2 = node.create_timer(1, None)
timer2 # noqa
assert 2 == len(node.timers)
assert node.destroy_timer(timer1) is True
assert 1 == len(node.timers)
try:
assert node.destroy_node() is True
except SystemError:
ret = False
else:
assert 0 == len(node.timers)
assert node.handle is None
ret = True
finally:
rclpy.shutdown()
return ret
def main(args=None):
rclpy.init(args)
node = rclpy.create_node("minimal_client_async")
cli = node.create_client(AddTwoInts, "add_two_ints")
req = AddTwoInts.Request()
req.a = 41
req.b = 1
# TODO(mikaelarguedas) remove this once wait for service implemented
# wait for connection to be established
# (no wait for service in Python yet)
time.sleep(1)
cli.call(req)
while rclpy.ok():
# TODO(mikaelarguedas) This is not the final API, and this does not scale
# for multiple pending requests. This will change once an executor model is implemented
# In the future the response will not be stored in cli.response
if cli.response is not None:
print("Result of add_two_ints: for %d + %d = %d" % (req.a, req.b, cli.response.sum))
break
rclpy.spin_once(node)
rclpy.shutdown()
def talker(message_name, number_of_cycles, namespace):
from test_msgs.message_fixtures import get_test_msg
import rclpy
message_pkg = 'test_msgs'
module = importlib.import_module(message_pkg + '.msg')
msg_mod = getattr(module, message_name)
rclpy.init(args=[])
node = rclpy.create_node('talker', namespace=namespace)
chatter_pub = node.create_publisher(
msg_mod, 'test/message/' + message_name)
cycle_count = 0
print('talker: beginning loop')
msgs = get_test_msg(message_name)
while rclpy.ok() and cycle_count < number_of_cycles:
msg_count = 0
for msg in msgs:
chatter_pub.publish(msg)
msg_count += 1
print('publishing message #%d' % msg_count)
time.sleep(0.01)
cycle_count += 1
time.sleep(0.1)
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_client_async')
cli = node.create_client(AddTwoInts, 'add_two_ints')
req = AddTwoInts.Request()
req.a = 41
req.b = 1
while not cli.wait_for_service(timeout_sec=1.0):
node.get_logger().info('service not available, waiting again...')
future = cli.call_async(req)
while rclpy.ok():
rclpy.spin_once(node)
if future.done():
if future.result() is not None:
node.get_logger().info(
'Result of add_two_ints: for %d + %d = %d' %
(req.a, req.b, future.result().sum))
else:
node.get_logger().info('Service call failed %r' % (future.exception(),))
break
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_client')
cli = node.create_client(AddTwoInts, 'add_two_ints')
req = AddTwoInts.Request()
req.a = 41
req.b = 1
# TODO(mikaelarguedas) remove this once wait for service implemented
# wait for connection to be established
# (no wait for service in Python yet)
time.sleep(1)
cli.call(req)
# when calling wait for future
# spin should not be called in the main loop
cli.wait_for_future()
# TODO(mikaelarguedas) This is not the final API, and this does not scale
# for multiple pending requests. This will change once an executor model is implemented
# In the future the response will not be stored in cli.response
print(
'Result of add_two_ints: for %d + %d = %d' %
(req.a, req.b, cli.response.sum))
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_publisher')
publisher = node.create_publisher(String, 'topic', 10)
msg = String()
i = 0
def timer_callback():
nonlocal i
msg.data = 'Hello World: %d' % i
i += 1
node.get_logger().info('Publishing: "%s"' % msg.data)
publisher.publish(msg)
timer_period = 0.5 # seconds
timer = node.create_timer(timer_period, timer_callback)
rclpy.spin(node)
# Destroy the timer attached to the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_timer(timer)
node.destroy_node()
rclpy.shutdown()
def talker(message_name, rmw_implementation, number_of_cycles):
import rclpy
from rclpy.qos import qos_profile_default
from rclpy.impl.rmw_implementation_tools import select_rmw_implementation
from message_fixtures import get_test_msg
message_pkg = 'test_communication'
module = importlib.import_module(message_pkg + '.msg')
msg_mod = getattr(module, message_name)
select_rmw_implementation(rmw_implementation)
rclpy.init([])
node = rclpy.create_node('talker')
chatter_pub = node.create_publisher(
msg_mod, 'test_message_' + message_name, qos_profile_default)
cycle_count = 0
print('talker: beginning loop')
msgs = get_test_msg(message_name)
while rclpy.ok() and cycle_count < number_of_cycles:
cycle_count += 1
msg_count = 0
for msg in msgs:
chatter_pub.publish(msg)
msg_count += 1
print('publishing message #%d' % msg_count)
time.sleep(0.1)
time.sleep(1)
rclpy.shutdown()
def cancel_done(self, future):
cancel_response = future.result()
if len(cancel_response.goals_canceling) > 0:
self.get_logger().info('Goal successfully canceled')
else:
self.get_logger().info('Goal failed to cancel')
rclpy.shutdown()
def func_init_shutdown():
import rclpy
try:
rclpy.init()
rclpy.shutdown()
return True
except RuntimeError:
return False
def main(args=None):
rclpy.init(args=args)
try:
talker = ThrottledTalker()
rclpy.spin(talker)
finally:
talker.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
minimal_service = MinimalService()
rclpy.spin(minimal_service)
rclpy.shutdown()
def main(argv=sys.argv):
rclpy.init(args=argv)
node = rclpy.create_node(
'initial_params_node', namespace='/', allow_undeclared_parameters=True)
rclpy.spin(node)
rclpy.shutdown()
return 0
def func_double_shutdown():
import rclpy
rclpy.init()
rclpy.shutdown()
try:
rclpy.shutdown()
except RuntimeError:
return True
return False
def get_result_callback(self, future):
result = future.result().result
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Goal succeeded! Result: {0}'.format(result.sequence))
else:
self.get_logger().info('Goal failed with status: {0}'.format(status))
# Shutdown after receiving a result
rclpy.shutdown()
def main(args=None):
rclpy.init(args)
node = rclpy.create_node('add_two_ints_server')
minimal_service = MinimalService(node)
minimal_service # prevent unused variable warning
while rclpy.ok():
rclpy.spin_once(node)
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
action_server = MinimalActionServer()
# We use a MultiThreadedExecutor to handle incoming goal requests concurrently
executor = MultiThreadedExecutor()
rclpy.spin(action_server, executor=executor)
action_server.destroy()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
minimal_publisher = MinimalPublisher()
rclpy.spin(minimal_publisher)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
minimal_publisher.destroy_node()
rclpy.shutdown()
def func_destroy_node():
import rclpy
rclpy.init()
node = rclpy.create_node('test_node1')
ret = True
try:
node.destroy_node()
except RuntimeError:
ret = False
finally:
rclpy.shutdown()
return ret
def main(args=None):
rclpy.init(args=args)
minimal_action_server = MinimalActionServer()
# Use a MultiThreadedExecutor to enable processing goals concurrently
executor = MultiThreadedExecutor()
rclpy.spin(minimal_action_server, executor=executor)
minimal_action_server.destroy()
rclpy.shutdown()
def node_fixture(request):
"""Create a fixture with a node and helper executable."""
rclpy.init()
node = rclpy.create_node('tests_yaml', allow_undeclared_parameters=True)
try:
yield {
'node': node,
'executable': request.param,
}
finally:
node.destroy_node()
rclpy.shutdown()
def func_corrupt_node_handle():
import rclpy
rclpy.init()
node = rclpy.create_node('test_node5')
try:
node.handle = 'garbage'
ret = False
except AttributeError:
node.destroy_node()
ret = True
finally:
rclpy.shutdown()
return ret
def main(args=None):
rclpy.init(args=args)
try:
executor = PriorityExecutor()
executor.add_high_priority_node(Estopper())
executor.add_node(Listener())
executor.add_node(Talker())
try:
executor.spin()
finally:
executor.shutdown()
finally:
rclpy.shutdown()
def func_destroy_corrupted_node():
import rclpy
rclpy.init()
node = rclpy.create_node('test_node2')
assert node.destroy_node() is True
node._handle = 'garbage'
ret = False
try:
node.destroy_node()
except SystemError:
ret = True
finally:
rclpy.shutdown()
return ret
def main(args=None):
rclpy.init(args)
node = rclpy.create_node('add_two_ints_server')
srv = node.create_service(AddTwoInts, 'add_two_ints', add_two_ints_callback)
while rclpy.ok():
rclpy.spin_once(node)
# Destroy the service attached to the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_service(srv)
rclpy.shutdown()
def main(args=None):
rclpy.init(args)
node = rclpy.create_node('minimal_publisher')
minimal_subscriber = MinimalSubscriber(node)
minimal_subscriber # prevent unused variable warning
while rclpy.ok():
rclpy.spin_once(node)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_node()
rclpy.shutdown()
def main(args=None):
"""
Run a Listener node standalone.
This function is called directly when using an entrypoint. Entrypoints are configured in
setup.py. This along with the script installation in setup.cfg allows a listener node to be run
with the command `ros2 run examples_rclpy_executors listener`.
:param args: Arguments passed in from the command line.
"""
rclpy.init(args=args)
try:
rclpy.spin(Listener())
finally:
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_subscriber')
subscription = node.create_subscription(
String, 'topic', lambda msg: print('I heard: [%s]' % msg.data))
subscription # prevent unused variable warning
rclpy.spin(node)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_subscriber')
subscription = node.create_subscription(String, 'topic', chatter_callback)
subscription # prevent unused variable warning
while rclpy.ok():
rclpy.spin_once(node)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
laser_to_pointcloud = laserToPointcloud()
rclpy.spin(laser_to_pointcloud)
laser_to_pointcloud.destroy_node()
rclpy.shutdown()
def tearDownClass(cls):
cls.node.destroy_node()
rclpy.shutdown(context=cls.context)
def main(args=None):
rclpy.init(args=args)
node = getCameraDataNode()
rclpy.spin(node)
rclpy.shutdown()
def main(args=None):
Bucket_list = []
Cube_list = []
rclpy.init(args=args)
ic = image_converter()
base = base_detection()
navpublisher = navigation_publisher()
classifier = image_classifier()
# Hack to get two images which clears the ghost images
while rclpy.ok() and ic.recievedImage is False:
rclpy.spin_once(ic)
ic.recievedImage = False
while rclpy.ok() and ic.recievedImage is False:
rclpy.spin_once(ic)
# getbase here takes in raw image to draw contours on
base.contourImg = ic.msg
base.getBase(ic.msg)
classifier.robot_ycord = base.robot_ycord
# After drawing base contours pass it off to draw classifications
classifier.contourImg = base.contourImg
classifier.classify_objects(ic.msg)
detection_array = classifier.detections
cv2.imshow("detections and base", classifier.contourImg)
cv2.waitKey(0)
for obj in detection_array:
# print("center Coordinates: ", obj.coordinateCenter)
obj.coordinateCenter = base.get_world_coordinates(obj.coordinateCenter[0],obj.coordinateCenter[1])
# print("world space coordinates: " , obj.coordinateCenter)
for obj in detection_array:
if isCube(obj) == False:
new_bucket = Bucket(center_x = obj.coordinateCenter[0],
center_y = obj.coordinateCenter[1],
classification= obj.classification)
Bucket_list.append(new_bucket)
else:
new_Cube = Cube(center_x = obj.coordinateCenter[0],
center_y = obj.coordinateCenter[1],
classification= obj.classification,
angle= obj.angle)
Cube_list.append(new_Cube)
print("start read environment")
#take in cubelist, bucketlist
print("start executeCommand")
# for obj in Cube_list:
# print("class: ", obj.classification)
# print("X: ", obj.center_x)
# print("Y: ", obj.center_y)
o_x = -3.139559138676179
o_y = -0.015483082006265326
first_run = True
for cube in Cube_list:
for bucket in Bucket_list:
if colorCheck(cube,bucket) == True:
#start navigation
print("moving cube", cube.classification, "into bucket", bucket.classification)
# If the angle is negative make it positive
if cube.angle < 0:
cube.angle * -1.0
o_z = (numpy.pi/2 - cube.angle) - numpy.pi/4
#initialize coordinates
cube.center_x = cube.center_x * -1.0
cube.center_y = cube.center_y * -1.0
bucket.center_x = bucket.center_x * -1.0
bucket.center_y = bucket.center_y * -1.0
#cube hover
print("Hover over cube position\n")
p_x = cube.center_x
p_y = cube.center_y
p_z = 1.10756253284
print("Going to", p_x, p_y)
if first_run:
navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 62.0, "open")
first_run = False
else:
navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 65.0, "open")
# cube position (grab)
print("Lowering position\n")
p_x = cube.center_x
p_y = cube.center_y
p_z = 1.040006
navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 65.0, "close")
# cube position (post-grab)
print("Rise up\n")
p_x = cube.center_x
p_y = cube.center_y
p_z = 1.20756253284
navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 65.0)
# bucket position hover
print("Hover over bucket\n")
p_x = bucket.center_x
p_y = bucket.center_y
p_z = 1.20756253284
navpublisher.publish_coordinates(p_x,p_y,p_z,o_x,o_y,o_z, 65.0, "open")
# Return home
navpublisher.publish_coordinates(0.0, 0.0, 2.1, 0.0, 0.0, 0.0, 62.0)
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = NumberPublisherNode()
rclpy.spin(node)
rclpy.shutdown()
def callback_kill(self, msg): del self self.destroy_node() rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = my_node()
rclpy.spin(node)
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node("my_first_node")
print("Started the node")
rclpy.spin(node)
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = TTSNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def __exit__(self, exc_type, exc_value, traceback):
self.node.destroy_node()
rclpy.shutdown()
def main (args=None):
rclpy.init(args=args)
node1=my_server()
rclpy.spin(node1)
rclpy.shutdown()
def destroy(self):
"""
Shuts down ROS node
"""
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node1 = stopAtWallNode()
rclpy.spin(node1)
rclpy.shutdown()
def main():
"""
Entry point for the tree watcher script.
"""
####################
# Arg Parsing
####################
# command_line_args = rclpy.utilities.remove_ros_args(command_line_args)[1:]
command_line_args = None
parser = command_line_argument_parser(formatted_for_sphinx=False)
args = parser.parse_args(command_line_args)
# mode is None if the user didn't specify any option in the exclusive group
if args.mode is None:
args.mode = py_trees_ros.trees.WatcherMode.SNAPSHOTS
args.snapshot_period = 2.0 if (args.statistics or args.blackboard_data or args.blackboard_activity) else py_trees.common.Duration.INFINITE.value
tree_watcher = py_trees_ros.trees.Watcher(
namespace_hint=args.namespace_hint,
topic_name=args.topic_name,
parameters=py_trees_ros.trees.SnapshotStream.Parameters(
blackboard_data=args.blackboard_data,
blackboard_activity=args.blackboard_activity,
snapshot_period=args.snapshot_period
),
mode=args.mode,
statistics=args.statistics,
)
####################
# Setup
####################
rclpy.init(args=None)
try:
tree_watcher.setup(timeout_sec=5.0)
# setup discovery fails
except py_trees_ros.exceptions.NotFoundError as e:
print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset)
sys.exit(1)
# setup discovery finds duplicates
except py_trees_ros.exceptions.MultipleFoundError as e:
print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset)
if args.namespace is None:
print(console.red + "\nERROR: select one with the --namespace argument\n" + console.reset)
else:
print(console.red + "\nERROR: but none matching the requested '{}'\n".format(args.namespace) + console.reset)
sys.exit(1)
except py_trees_ros.exceptions.TimedOutError as e:
print(console.red + "\nERROR: {}\n".format(str(e)) + console.reset)
sys.exit(1)
####################
# Execute
####################
executor = rclpy.executors.SingleThreadedExecutor()
executor.add_node(node=tree_watcher.node)
try:
while True:
if not rclpy.ok():
break
if tree_watcher.done:
if tree_watcher.xdot_process is None:
# no xdot found on the system, just break out and finish
break
elif tree_watcher.xdot_process.poll() is not None:
# xdot running, wait for it to terminate
break
executor.spin_once(timeout_sec=0.1)
except KeyboardInterrupt:
pass
finally:
if tree_watcher.xdot_process is not None:
if tree_watcher.xdot_process.poll() is not None:
tree_watcher.xdot_process.terminate()
tree_watcher.shutdown()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = RobotNewsStationNode()
rclpy.spin(node)
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
fire_alarm = fire_alarm_sound()
rclpy.spin(fire_alarm)
fire_alarm.destroy_node()
rclpy.shutdown()
def shutdown(self):
self._executor.shutdown()
rclpy.shutdown(context=self._context)
self.destroy_node()
self._thread.join()
def executor(self):
while rclpy.ok():
rclpy.spin_once(self.kuka_node)
self.kuka_node.destroy_node()
rclpy.shutdown()
def main():
rclpy.init()
odom = WheelOdom()
rclpy.spin(odom)
rclpy.shutdown()
def socket(self, ip, port):
self.UDPsocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# Bind the socket to the port
server_address = (ip, port)
#TEST:
local_port = 20001
local_hostname = socket.gethostname()
local_ip = socket.gethostbyname(local_hostname)
server_address = (local_ip, local_port)
os.system('clear')
print(cl_pink('==========================================\n'))
print(cl_cyan('Starting up on:'), 'IP:', local_ip, 'Port:', local_port)
try:
self.UDPsocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.UDPsocket.bind(server_address)
except:
print(cl_red('Error: ') + "Connection for KUKA cannot assign requested address:", ip, port)
os._exit(-1)
# Wait for a connection
print(cl_cyan('Waiting for a connection...'))
data, self.client_address = self.UDPsocket.recvfrom(self.BUFFER_SIZE)
print(cl_cyan('Connection from: '), self.client_address)
print(cl_cyan('Message: '), data)
self.UDPsocket.sendto("HALLA".encode(), self.client_address)
print("Responded KUKA")
self.isconnected = True
last_read_time = time.time()
while self.isconnected:
try:
data,addr = self.UDPsocket.recvfrom(self.BUFFER_SIZE)
###### TEST DECODE #######
#data.decode("utf-8")
data = data.decode()
##########################
last_read_time = time.time() # Keep received time
# Process the received data package
for pack in data.split(">"): # parsing data pack
cmd_splt = pack.split()
if len(pack) and cmd_splt[0] == 'Joint_Pos': # If it's JointPosition
tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
if len(tmp) == 7: self.JointPosition = (tmp, last_read_time)
if len(pack) and cmd_splt[0] == 'Tool_Pos': # If it's ToolPosition
tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
if len(tmp) == 6: self.ToolPosition = (tmp, last_read_time)
if len(pack) and cmd_splt[0] == 'Tool_Force': # If it's ToolForce
tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
if len(tmp) == 3:
self.ToolForce = (tmp, last_read_time)
if len(pack) and cmd_splt[0] == 'Tool_Torque': # If it's ToolTorque
tmp = [float(''.join([c for c in s if c in '0123456789.eE-'])) for s in cmd_splt[1:]]
if len(tmp) == 3:
self.ToolTorque = (tmp, last_read_time)
if len(pack) and cmd_splt[0] == 'isCompliance': # If isCompliance
if cmd_splt[1] == "false":
self.isCompliance = (False, last_read_time)
elif cmd_splt[1] == "true":
self.isCompliance = (True, last_read_time)
if len(pack) and cmd_splt[0] == 'isCollision': # If isCollision
if cmd_splt[1] == "false":
self.isCollision = (False, last_read_time)
elif cmd_splt[1] == "true":
self.isCollision = (True, last_read_time)
if len(pack) and cmd_splt[0] == 'isReadyToMove': # If isReadyToMove
if cmd_splt[1] == "false":
self.isReadyToMove = (False, last_read_time)
elif cmd_splt[1] == "true":
self.isReadyToMove = (True, last_read_time)
if len(pack) and cmd_splt[0] == 'isMastered': # If isMastered
if cmd_splt[1] == "false":
self.isMastered = (False, last_read_time)
elif cmd_splt[1] == "true":
self.isMastered = (True, last_read_time)
if len(pack) and cmd_splt[0] == 'OperationMode': # If OperationMode
self.OperationMode = (cmd_splt[1], last_read_time)
if len(pack) and cmd_splt[0] == 'JointAcceleration': # If it's JointAcceleration
self.JointAcceleration = (float(cmd_splt[1]), last_read_time)
if len(pack) and cmd_splt[0] == 'JointVelocity': # If it's JointVelocity
self.JointVelocity = (float(cmd_splt[1]), last_read_time)
if len(pack) and cmd_splt[0] == 'JointJerk': # If it's JointJerk
self.JointJerk = (float(cmd_splt[1]), last_read_time)
if len(pack) and cmd_splt[0] == 'isFinished': # If isFinished
if cmd_splt[1] == "false":
self.isFinished = (False, last_read_time)
elif cmd_splt[1] == "true":
self.isFinished = (True, last_read_time)
if len(pack) and cmd_splt[0] == 'hasError': # If hasError
if cmd_splt[1] == "false":
self.hasError = (False, last_read_time)
elif cmd_splt[1] == "true":
self.hasError = (True, last_read_time)
if (all(item != None for item in self.JointPosition[0]) and
all(item != None for item in self.ToolPosition[0]) and
all(item != None for item in self.ToolForce[0]) and
all(item != None for item in self.ToolTorque[0]) and
self.isCompliance[1] != None and
self.isCollision[1] != None and
self.isReadyToMove[1] != None and
self.isMastered[1] != None and
self.OperationMode[1] != None):
# self.OprationMode = (None, None)
self.isready = True
else:
self.isready = False
except:
elapsed_time = time.time() - last_read_time
if elapsed_time > 5.0: # Didn't receive a pack in 5s
print("exception!!")
self.close() # Disconnect from iiwa
self.isconnected = False
print(cl_lightred('No packet received from iiwa for 5s!'))
print("SHUTTING DOWN")
#self.connection.shutdown(socket.SHUT_RDWR)
#self.connection.close()
self.UDPsocket.close()
self.isconnected = False
print(cl_lightred('Connection is closed!'))
# NOE JEG SLANG P SELV :`)
rclpy.shutdown()
def main(args: Any = None) -> None:
rclpy.init(args=args)
thymio_driver = ThymioDriver(namespace='', standalone=True)
rclpy.spin(thymio_driver)
thymio_driver.destroy_node()
rclpy.shutdown()
def tearDown(self):
self.node.destroy_node()
self.executor.shutdown()
rclpy.shutdown(context=self.context)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--max-number-of-actions",
"-a",
type=int,
required=True,
help="""Maximum numbers of actions that are processed. After this the
backend shuts down automatically.
""",
)
camera_group = parser.add_mutually_exclusive_group()
camera_group.add_argument(
"--cameras",
"-c",
action="store_true",
help="Run camera backend.",
)
camera_group.add_argument(
"--cameras-with-tracker",
action="store_true",
help="Run camera backend with integrated object tracker.",
)
parser.add_argument(
"--robot-logfile",
type=str,
help="""Path to a file to which the robot data log is written. If not
specified, no log is generated.
""",
)
parser.add_argument(
"--camera-logfile",
type=str,
help="""Path to a file to which the camera data is written. If not
specified, no log is generated.
""",
)
args = parser.parse_args()
rclpy.init()
node = NotificationNode("trifinger_data")
logger = node.get_logger()
cameras_enabled = False
if args.cameras:
cameras_enabled = True
from trifinger_cameras import tricamera
elif args.cameras_with_tracker:
cameras_enabled = True
import trifinger_object_tracking.py_tricamera_types as tricamera
if cameras_enabled:
logger.info("Start camera data")
camera_data = tricamera.MultiProcessData("tricamera", True,
CAMERA_TIME_SERIES_LENGTH)
logger.info("Start robot data")
# Storage for all observations, actions, etc.
robot_data = robot_interfaces.trifinger.MultiProcessData(
"trifinger", True, history_size=ROBOT_TIME_SERIES_LENGTH)
if args.robot_logfile:
robot_logger = robot_interfaces.trifinger.Logger(
robot_data, buffer_limit=args.max_number_of_actions)
robot_logger.start()
if cameras_enabled and args.camera_logfile:
camera_fps = 10
robot_rate_hz = 1000
# make the logger buffer a bit bigger as needed to be on the safe side
buffer_length_factor = 1.5
episode_length_s = args.max_number_of_actions / robot_rate_hz
# Compute camera log size based on number of robot actions plus some
# buffer
log_size = int(camera_fps * episode_length_s * buffer_length_factor)
logger.info("Initialize camera logger with buffer size %d" % log_size)
camera_logger = tricamera.Logger(camera_data, log_size)
logger.info("Data backend is ready")
# send ready signal
node.publish_status("READY")
if cameras_enabled and args.camera_logfile:
# wait for first action to be sent by the user (but make sure to not
# block when shutdown is requested)
while (not robot_data.desired_action.wait_for_timeindex(
0, max_duration_s=1) and not node.shutdown_requested):
rclpy.spin_once(node, timeout_sec=0)
camera_logger.start()
logger.info("Start camera logging")
while not node.shutdown_requested:
rclpy.spin_once(node)
logger.debug("Received shutdown signal")
if cameras_enabled and args.camera_logfile:
logger.info("Save recorded camera data to file %s" %
args.camera_logfile)
camera_logger.stop_and_save(args.camera_logfile)
if args.robot_logfile:
logger.info("Save robot data to file %s" % args.robot_logfile)
robot_logger.stop_and_save(
args.robot_logfile,
robot_interfaces.trifinger.Logger.Format.BINARY)
rclpy.shutdown()
return 0
def get_result_callback(self, future):
result = future.result().result
self.get_logger().warn(f'Action Done !! Result: {result.sequence}')
rclpy.shutdown()
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_thrusters_output_efficiency',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Set the thruster output efficiency for vehicle, namespace=' + self.get_namespace())
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# node.set_parameters([sim_time])
starting_time = 0.0
if node.has_parameter('starting_time'):
starting_time = float(node.get_parameter('starting_time').value)
node.get_logger().info('Starting time= {} s'.format(starting_time))
duration = 0.0
if node.has_parameter('duration'):
duration = float(node.get_parameter('duration').value)
if duration <= 0.0:
raise RuntimeError('Duration not set, leaving node...')
node.get_logger().info('Duration [s]=', ('Inf.' if duration < 0 else str(duration)))
if node.has_parameter('efficiency'):
efficiency = float(node.get_parameter('efficiency').value)
if efficiency < 0 or efficiency > 1:
raise RuntimeError('Invalid thruster output efficiency, leaving node...')
else:
raise RuntimeError('Thruster output efficiency not set, leaving node...')
thruster_id = -1
if node.has_parameter('thruster_id'):
if node.get_parameter('thruster_id').type_ == rclpy.Parameter.Type.INTEGER:
thruster_id = node.get_parameter('thruster_id').get_parameter_value().integer_value
else:
raise RuntimeError('Thruster ID not given')
if thruster_id < 0:
raise RuntimeError('Invalid thruster ID')
node.get_logger().info('Setting thruster output efficiency #{} to {}'.format(thruster_id, 100 * efficiency))
vehicle_name = node.get_namespace().replace('/', '')
srv_name = build_service_name(vehicle_name, thruster_id, 'set_thrust_force_efficiency')
set_eff = node.create_client(SetThrusterEfficiency, srv_name)
if not set_eff.wait_for_service(timeout_sec=2):
raise RuntimeError('Service %s not available! Closing node...' %(srv_name))
FREQ = 100
rate = node.create_rate(FREQ)
thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
thread.start()
while time_in_float_sec(node.get_clock().now()) < starting_time:
#Just a guard for really short timeouts
if 1.0 / FREQ < starting_time:
rate.sleep()
# rate = node.create_rate(100)
# while time_in_float_sec(node.get_clock().now()) < starting_time:
# rate.sleep()
req = SetThrusterEfficiency.Request()
req.efficiency = efficiency
#success = set_eff.call(efficiency)
future = set_eff.call_async(efficiency)
#NB : spining is done from another thread
while not future.done():
try:
response = future.result()
except Exception as e:
node.get_logger().info('Service call ' + srv_name + ' failed, error=' + str(e)):
else:
node.get_logger().info('Time={} s'.format(rtime_in_float_sec(node.get_clock().now())))
node.get_logger().info('Current thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))
if duration > 0:
while time_in_float_sec(node.get_clock().now()) < starting_time + duration:
if 1.0 / FREQ < starting_time + duration:
rate.sleep()
# rate = node.create_rate.Rate(100)
# while time_in_float_sec(node.get_clock().now()) < starting_time + duration:
# rate.sleep()
req.efficiency = 1.0
success = set_eff.call(efficiency)
while not future.done():
try:
response = future.result()
except Exception as e:
node.get_logger().info('Service call ' + srv_name + ' failed, error=' + str(e)):
else:
node.get_logger().info('Time={} s'.format(time_in_float_sec(node.get_clock().now())))
node.get_logger().infoint(
'Returning to previous thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))
# if success:
# print('Time={} s'.format(time_in_float_sec(node.get_clock().now())))
# print('Returning to previous thruster output efficiency #{}={}'.format(thruster_id, efficiency * 100))
node.get_logger().info('Leaving node...')
node.destroy_node()
rclpy.shutdown()
thread.join()
def tearDownClass(cls):
cls.node.destroy_node()
rclpy.shutdown()
def main(stdscr):
rclpy.init()
node = RoompiKeyTeleop(MainWindow(stdscr))
rclpy.spin(node)
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
command_node = CVCommandsNode()
rclpy.spin(command_node)
rclpy.shutdown()
def teardown_module(module):
console.banner("ROS Shutdown")
rclpy.shutdown()
# アクションのキャンセル
goal_handle.set_canceled()
self.get_logger().info('goal canceled')
return Fibonacci.Result()
# フィボナッチ数列の更新
msg.sequence.append(msg.sequence[i] + msg.sequence[i - 1])
self.get_logger().info('feedback: {0}'.format(msg.sequence))
# アクションのフィードバックの送信
goal_handle.publish_feedback(msg)
# 1秒待機（重たい処理の代わり）
time.sleep(1)
# アクションの実行結果の送信
goal_handle.set_succeeded()
result = Fibonacci.Result()
result.sequence = msg.sequence
self.get_logger().info('result: {0}'.format(result.sequence))
return result
if __name__ == '__main__':
rclpy.init(args=args)
minimal_action_server = MinimalActionServer()
# マルチスレッドでminimal_action_serverノードを実行し、
# 複数のアクションクライアントを同時処理
executor = MultiThreadedExecutor()
rclpy.spin(minimal_action_server, executor=executor)
minimal_action_server.destroy()
rclpy.shutdown()