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 main(argv=sys.argv[1:]):
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
'--reliable', dest='reliable', action='store_true',
help='set qos profile to reliable')
parser.set_defaults(reliable=False)
parser.add_argument(
'-n', '--number_of_cycles', type=int, default=20,
help='number of sending attempts')
args = parser.parse_args(argv)
rclpy.init()
if args.reliable:
custom_qos_profile = qos_profile_default
print('Reliable publisher')
else:
custom_qos_profile = qos_profile_sensor_data
print('Best effort publisher')
node = rclpy.create_node('talker_qos')
chatter_pub = node.create_publisher(String, 'chatter_qos', custom_qos_profile)
msg = String()
cycle_count = 0
while rclpy.ok() and cycle_count < args.number_of_cycles:
msg.data = 'Hello World: {0}'.format(cycle_count)
print('Publishing: "{0}"'.format(msg.data))
chatter_pub.publish(msg)
cycle_count += 1
sleep(1)
def main(argv=sys.argv[1:]):
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
'--reliable', dest='reliable', action='store_true',
help='set qos profile to reliable')
parser.set_defaults(reliable=False)
parser.add_argument(
'-n', '--number_of_cycles', type=int, default=20,
help='max number of spin_once iterations')
args = parser.parse_args(argv)
rclpy.init()
if args.reliable:
custom_qos_profile = qos_profile_default
print('Reliable listener')
else:
custom_qos_profile = qos_profile_sensor_data
print('Best effort listener')
node = rclpy.create_node('listener_qos')
sub = node.create_subscription(String, 'chatter_qos', chatter_callback, custom_qos_profile)
assert sub # prevent unused warning
cycle_count = 0
while rclpy.ok() and cycle_count < args.number_of_cycles:
rclpy.spin_once(node)
cycle_count += 1
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 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 func_destroy_node_twice():
import rclpy
rclpy.init()
node = rclpy.create_node('test_node2')
assert node.destroy_node() is True
assert node.destroy_node() is True
return True
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 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 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 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 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)
action_client = MinimalActionClient()
action_client.send_goal()
rclpy.spin(action_client)
def _rostopic_echo_test(topic):
rclpy.init(None)
node = rclpy.create_node('rostopic')
# TODO FIXME no path resolving for topics yet implemented thereby the initial "/" is
# omited.
sub = node.create_subscription(String, topic[1:], chatter_callback, qos_profile_default)
while rclpy.ok():
rclpy.spin_once(node)
def main(args=None):
rclpy.init(args=args)
minimal_service = MinimalService()
rclpy.spin(minimal_service)
rclpy.shutdown()
def func_init_shutdown():
import rclpy
try:
rclpy.init()
rclpy.shutdown()
return True
except RuntimeError:
return False
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 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)
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)
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 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 main(args=None):
if args is None:
args = sys.argv
rclpy.init(args)
node = rclpy.create_node('listener')
sub = node.create_subscription(String, 'chatter', chatter_callback, qos_profile_default)
assert sub # prevent unused warning
rclpy.spin(node)
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 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_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)
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 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=args)
drive = Drive()
rclpy.spin(drive)
def main(args=None):
rclpy.init(args=args)
publisher = PointTrajectoryPublisher()
rclpy.spin(publisher)
publisher.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = AddTwoIntsServerNode()
rclpy.spin(node)
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
random_control = RandomControl()
rclpy.spin(random_control)
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self._rclpy_context = rclpy.context.Context()
rclpy.init(args=self._launch_file_arguments, context=self._rclpy_context)
def main(args=None):
rclpy.init(args=args)
fibonacci_action_server = FibonacciActionServer()
rclpy.spin(fibonacci_action_server)
def main(args=None):
rclpy.init(args=args)
node = RobotNewsStationNode()
rclpy.spin(node)
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args) # initialyse ROS2 communication
node=Node('number_publisher')
node.get_logger().info("Hello ROS2")
rclpy.shutdown() # close ROS2 communications
def main(args=None):
rclpy.init(args=args)
calib = CameraCalibration('camera_calibration', args=args)
rclpy.spin(calib)
calib.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
minimal_service = MinimalService()
rclpy.spin(minimal_service)
rclpy.shutdown()
def main():
# 初始化 ros2 python - rclpy & 外置参数引入
zed = YF_Zed2()
init()
rclpy.init()
# 构建相关节点
flag = yf_node.YF_ObjectFlag(nodeName['ObjectFlag'],'ObjectFlag')
video = yf_node.YF_Image_PY(nodeName['Video'],'Video')
goal = yf_node.YF_Goal(nodeName['Goal'],'Goal')
cost = yf_node.YF_CostMap(nodeName["CostMap"],"CostMap")
showMap = yf_node.YF_CompressedImage(nodeName["ShowMap"],"ShowMap") # YF_CompressedImage YF_Image_PY
# set init flag = 101
flag.publishMsg(101)
# 广播节点的首次初始化
rclpy.spin_once(flag.node,timeout_sec=0.001)
rclpy.spin_once(goal.node,timeout_sec=0.001)
rclpy.spin_once(video.node,timeout_sec=0.001)
rclpy.spin_once(cost.node,timeout_sec=0.001)
rclpy.spin_once(showMap.node,timeout_sec=0.001)
# init a time point for fps
t = time.time()
liveVideo = None
target = None
old_target = [0,0]
objectCache = None
videoCache = None
frozenFrame = False
trackTarget = False
while 1:
# 运行数据捕捉: become target only for debug
zed.getData()
rclpy.spin_once(flag.node,timeout_sec=0.001)
signal = flag.subMsg.data.tolist()
liveImage = zed.liveImage
objectArray = zed.objectArray
poc = zed.pointCloud
pcl = PointCloud2_Img2Array(poc)
# 等待捕获所有信息
if liveImage is None:
print("Waiting for new Image")
time.sleep(0.2)
continue
if objectArray is None:
print("Waiting for new Object")
time.sleep(0.2)
continue
print(signal)
if signal == 101:
liveVideo,_ = runObjectDetection(liveImage, objectArray)
elif signal == 0 and not frozenFrame:
videoCache = liveImage
liveVideo,objectCache = runObjectDetection(liveImage, objectArray)
if len(objectCache) == 0:
flag.publishMsg(101)
continue
flag.publishMsg(-1*len(objectCache))
frozenFrame = True
trackTarget = False
elif signal == 0 and frozenFrame:
continue
elif 100 >= signal > 0 and not trackTarget:
frozenFrame = False
trackTarget = True
objIdx = flag.subMsg.tolist()-1
obj = objectCache[objIdx]
runTargetSet(obj,videoCache,goal)
elif 100 >= signal > 0 and trackTarget:
target,liveVideo = runTrack(liveImage, objectArray, goal,flag)
else:
print("Waiting User select target")
continue
video.publishMsg(liveVideo.astype(np.uint8))
# 获得地图
if target is None:
target = old_target
else:
old_target = target
obMap,sendMap = get_obMap(pcl,poc,target,CostMap,showImg)
# 广播地图
cost.publishMsg(obMap.flatten().astype(np.uint8))
showMap.publishMsg(sendMap.astype(np.uint8))
# print fps
framePerSecond = int(1/(time.time()-t))
# print("LiveVideo Fps: ", framePerSecond)
t = time.time()
rclpy.spin_once(flag.node,timeout_sec=0.001)
rclpy.spin_once(video.node,timeout_sec=0.001)
rclpy.spin_once(cost.node,timeout_sec=0.001)
rclpy.spin_once(showMap.node,timeout_sec=0.001)
# print live video
if FPS:
cv2.putText(liveVideo, "FPS:{}".format(framePerSecond),org=(5,25),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,fontScale=0.8,
color=(0,255,0),thickness=2)
if showImg[2]:
cv2.imshow("LiveVideo",liveVideo)
# 中断守护
if cv2.waitKey(25) & 0xFF == ord('q'):
cv2.destroyAllWindows()
break
# 杀死所有订阅节点
video.node.destroy_node()
cost.node.destroy_node()
showMap.node.destroy_node()
goal.node.destroy_node()
flag.node.destroy_node()
zed.zed2.close()
# 结束rclpy
rclpy.shutdown()
def main():
rclpy.init()
parameter = Paremeter()
rclpy.spin(parameter)
parameter.destroy_node() # destory node explicitly
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = Keyboard_Input()
rclpy.spin(node)
def main(args=None):
rclpy.init(args=args)
action_client = MinimalActionClient()
action_client.send_goal()
rclpy.spin(action_client)
action_client.destroy_node()
def __init__(self):
# Launch the simulation with the given launchfile name
rclpy.init(args=None)
self.node = rclpy.create_node('real_env_ros2')
def main(args=None):
rclpy.init(args=args)
server = ServiceServer()
rclpy.spin(server)
rclpy.shutdown()
def main(args=None):
global steering_angle
global steer_mode
global desire_angle
rclpy.init()
node = rclpy.create_node('steer_PID')
steerPub = node.create_publisher(Int16, '/dbw_cmd/Steer', qos_profile_default)
thread_velo = threading.Thread(target=execute, args=(node,))
thread_velo.start()
thread_steer = threading.Thread(target=steerfunction, args=(steerPub,))
thread_steer.start()
handle_set = 0
status = 0
handle_set_MAX = 440
try:
print(msg)
while(1):
if abs(handle_set) >= handle_set_MAX:
Hsigned = -1 if handle_set < 0 else 1
handle_set = Hsigned * handle_set_MAX
#print('='*30,"\n")
key = getKey()
os.system('cls' if os.name == 'nt' else 'clear')
#print('\n\n\n\n\n','='*30, sep='')
if key in steerControl.keys():
if key == "h" :
steer_mode = steerControl[key]
if key == "j" :
steer_mode = steerControl[key]
if key == "u" :
desire_angle += steerControl[key]
if key == "n" :
desire_angle += steerControl[key]
elif key in moveBindings.keys():
if key == "q" :
handle_set = 0
handle_set += moveBindings[key]
else:
print("BRACE!! EMERGENCY STOP!!!\n"*3)
###MUST be ON THREAD!!!!
os.system('''for k in {1..3};
do
play -nq -t alsa synth 0.2 sine 544;
sleep 0.03;
play -nq -t alsa synth 0.2 sine 544;
sleep 0.03;
play -nq -t alsa synth 0.2 sine 544;
sleep 0.03;
play -nq -t alsa synth 0.2 sine 544;
sleep 0.03;
done &''')
if (key == '\x03'):
break
print("steer_mode = ", steer_mode , " , desire_angle = ", desire_angle)
steer = Int16()
steer.data = handle_set
if steer_mode == 0:
steerPub.publish(steer)
except Exception as e:
print(e)
finally:
steer = Int16()
steer = 0
steerPub.publish(steer)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def main(args=None):
rclpy.init(args=args)
warehouse_controller = WarehouseController()
warehouse_controller.create_test_plan()
warehouse_controller.execute_plan()
def main():
rclpy.init()
navigator = BasicNavigator()
# Set our demo's initial pose
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = 3.45
initial_pose.pose.position.y = 2.15
initial_pose.pose.orientation.z = 1.0
initial_pose.pose.orientation.w = 0.0
navigator.setInitialPose(initial_pose)
# Activate navigation, if not autostarted. This should be called after setInitialPose()
# or this will initialize at the origin of the map and update the costmap with bogus readings.
# If autostart, you should `waitUntilNav2Active()` instead.
# navigator.lifecycleStartup()
# Wait for navigation to fully activate, since autostarting nav2
navigator.waitUntilNav2Active()
# If desired, you can change or load the map as well
# navigator.changeMap('/path/to/map.yaml')
# You may use the navigator to clear or obtain costmaps
# navigator.clearAllCostmaps() # also have clearLocalCostmap() and clearGlobalCostmap()
# global_costmap = navigator.getGlobalCostmap()
# local_costmap = navigator.getLocalCostmap()
# Go to our demos first goal pose
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = -2.0
goal_pose.pose.position.y = -0.5
goal_pose.pose.orientation.w = 1.0
# sanity check a valid path exists
# path = navigator.getPath(initial_pose, goal_pose)
navigator.goToPose(goal_pose)
i = 0
while not navigator.isTaskComplete():
################################################
#
# Implement some code here for your application!
#
################################################
# Do something with the feedback
i = i + 1
feedback = navigator.getFeedback()
if feedback and i % 5 == 0:
print('Estimated time of arrival: ' + '{0:.0f}'.format(
Duration.from_msg(
feedback.estimated_time_remaining).nanoseconds / 1e9) +
' seconds.')
# Some navigation timeout to demo cancellation
if Duration.from_msg(
feedback.navigation_time) > Duration(seconds=600.0):
navigator.cancelTask()
# Some navigation request change to demo preemption
if Duration.from_msg(
feedback.navigation_time) > Duration(seconds=18.0):
goal_pose.pose.position.x = -3.0
navigator.goToPose(goal_pose)
# Do something depending on the return code
result = navigator.getResult()
if result == TaskResult.SUCCEEDED:
print('Goal succeeded!')
elif result == TaskResult.CANCELED:
print('Goal was canceled!')
elif result == TaskResult.FAILED:
print('Goal failed!')
else:
print('Goal has an invalid return status!')
navigator.lifecycleShutdown()
exit(0)
from std_msgs.msg import String
class MinimalPublisher(Node):
def __init__(self):
super().__init__('minimal_publisher')
# String型のchatterトピックを送信するpublisherの定義
self.publisher = self.create_publisher(String, 'chatter')
# 送信周期毎にtimer_callbackを呼び出し（送信周期は0.5秒）
self.timer = self.create_timer(0.5, self.timer_callback)
def timer_callback(self):
msg = String()
msg.data = 'Hello World!'
# chatterトピックにmsgを送信
self.publisher.publish(msg)
# msgの中身を標準出力にログ
self.get_logger().info(msg.data)
if __name__ == '__main__':
# Pythonクライアントライブラリの初期化
rclpy.init(args=args)
# minimal_publisherノードの作成
minimal_publisher = MinimalPublisher()
# minimal_publisherノードの実行開始
rclpy.spin(minimal_publisher)
# Pythonクライアントライブラリの終了
rclpy.shutdown()
def main (args=None):
rclpy.init(args=args)
node=my_node()
rclpy.spin(node )
rclpy.shutdown()
def __init__(self):
"""
Initialize the MARA environemnt
"""
# Manage command line args
args = ut_generic.getArgsParserMARA().parse_args()
self.gzclient = args.gzclient
self.realSpeed = args.realSpeed
self.velocity = args.velocity
self.multiInstance = args.multiInstance
self.port = args.port
# Set the path of the corresponding URDF file
if self.realSpeed:
urdf = "reinforcement_learning/mara_robot_gripper_140_camera_run.urdf"
urdfPath = get_prefix_path("mara_description") + "/share/mara_description/urdf/" + urdf
else:
urdf = "reinforcement_learning/mara_robot_gripper_140_camera_train.urdf"
urdfPath = get_prefix_path("mara_description") + "/share/mara_description/urdf/" + urdf
# Launch mara in a new Process
self.launch_subp = ut_launch.startLaunchServiceProcess(
ut_launch.generateLaunchDescriptionMara(
self.gzclient, self.realSpeed, self.multiInstance, self.port, urdf))
# Wait a bit for the spawn process.
# TODO, replace sleep function.
time.sleep(5)
# Create the node after the new ROS_DOMAIN_ID is set in generate_launch_description()
rclpy.init(args=None)
self.node = rclpy.create_node(self.__class__.__name__)
# class variables
self._observation_msg = None
self.max_episode_steps = 1024 #default value, can be updated from baselines
self.iterator = 0
self.reset_jnts = True
self._collision_msg = None
#############################
# Environment hyperparams
#############################
# Target, where should the agent reach
self.targetPosition = np.asarray([-0.40028, 0.095615, 0.72466]) # close to the table
self.target_orientation = np.asarray([0., 0.7071068, 0.7071068, 0.]) # arrow looking down [w, x, y, z]
# self.targetPosition = np.asarray([-0.386752, -0.000756, 1.40557]) # easy point
# self.target_orientation = np.asarray([-0.4958324, 0.5041332, 0.5041331, -0.4958324]) # arrow looking opposite to MARA [w, x, y, z]
EE_POINTS = np.asmatrix([[0, 0, 0]])
EE_VELOCITIES = np.asmatrix([[0, 0, 0]])
# Initial joint position
INITIAL_JOINTS = np.array([0., 0., 0., 0., 0., 0.])
# # Topics for the robot publisher and subscriber.
JOINT_PUBLISHER = '/mara_controller/command'
JOINT_SUBSCRIBER = '/mara_controller/state'
# joint names:
MOTOR1_JOINT = 'motor1'
MOTOR2_JOINT = 'motor2'
MOTOR3_JOINT = 'motor3'
MOTOR4_JOINT = 'motor4'
MOTOR5_JOINT = 'motor5'
MOTOR6_JOINT = 'motor6'
EE_LINK = 'ee_link'
# Set constants for links
WORLD = 'world'
TABLE = 'table'
BASE = 'base_link'
MARA_MOTOR1_LINK = 'motor1_link'
MARA_MOTOR2_LINK = 'motor2_link'
MARA_MOTOR3_LINK = 'motor3_link'
MARA_MOTOR4_LINK = 'motor4_link'
MARA_MOTOR5_LINK = 'motor5_link'
MARA_MOTOR6_LINK = 'motor6_link'
EE_LINK = 'ee_link'
JOINT_ORDER = [MOTOR1_JOINT,MOTOR2_JOINT, MOTOR3_JOINT,
MOTOR4_JOINT, MOTOR5_JOINT, MOTOR6_JOINT]
LINK_NAMES = [ WORLD, TABLE, BASE,
MARA_MOTOR1_LINK, MARA_MOTOR2_LINK,
MARA_MOTOR3_LINK, MARA_MOTOR4_LINK,
MARA_MOTOR5_LINK, MARA_MOTOR6_LINK, EE_LINK]
reset_condition = {
'initial_positions': INITIAL_JOINTS,
'initial_velocities': []
}
#############################
m_jointOrder = copy.deepcopy(JOINT_ORDER)
m_linkNames = copy.deepcopy(LINK_NAMES)
# Initialize target end effector position
self.environment = {
'jointOrder': m_jointOrder,
'linkNames': m_linkNames,
'reset_conditions': reset_condition,
'tree_path': urdfPath,
'end_effector_points': EE_POINTS,
}
# Subscribe to the appropriate topics, taking into account the particular robot
self._pub = self.node.create_publisher(JointTrajectory, JOINT_PUBLISHER, qos_profile=qos_profile_sensor_data)
self._sub = self.node.create_subscription(JointTrajectoryControllerState, JOINT_SUBSCRIBER, self.observation_callback, qos_profile=qos_profile_sensor_data)
self._sub_coll = self.node.create_subscription(ContactState, '/gazebo_contacts', self.collision_callback, qos_profile=qos_profile_sensor_data)
self.reset_sim = self.node.create_client(Empty, '/reset_simulation')
# Initialize a tree structure from the robot urdf.
# note that the xacro of the urdf is updated by hand.
# The urdf must be compiled.
_, self.ur_tree = tree_urdf.treeFromFile(self.environment['tree_path'])
# Retrieve a chain structure between the base and the start of the end effector.
self.mara_chain = self.ur_tree.getChain(self.environment['linkNames'][0], self.environment['linkNames'][-1])
self.numJoints = self.mara_chain.getNrOfJoints()
# Initialize a KDL Jacobian solver from the chain.
self.jacSolver = ChainJntToJacSolver(self.mara_chain)
self.obs_dim = self.numJoints + 6
# # Here idially we should find the control range of the robot. Unfortunatelly in ROS/KDL there is nothing like this.
# # I have tested this with the mujoco enviroment and the output is always same low[-1.,-1.], high[1.,1.]
low = -np.pi * np.ones(self.numJoints)
high = np.pi * np.ones(self.numJoints)
self.action_space = spaces.Box(low, high)
high = np.inf*np.ones(self.obs_dim)
low = -high
self.observation_space = spaces.Box(low, high)
# Spawn Target element in gazebo.
# node & spawn_cli initialized in super class
spawn_cli = self.node.create_client(SpawnEntity, '/spawn_entity')
while not spawn_cli.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info('/spawn_entity service not available, waiting again...')
modelXml = ut_gazebo.getTargetSdf()
pose = Pose()
pose.position.x = self.targetPosition[0]
pose.position.y = self.targetPosition[1]
pose.position.z = self.targetPosition[2]
pose.orientation.x = self.target_orientation[1]
pose.orientation.y= self.target_orientation[2]
pose.orientation.z = self.target_orientation[3]
pose.orientation.w = self.target_orientation[0]
#override previous spawn_request element.
self.spawn_request = SpawnEntity.Request()
self.spawn_request.name = "target"
self.spawn_request.xml = modelXml
self.spawn_request.robot_namespace = ""
self.spawn_request.initial_pose = pose
self.spawn_request.reference_frame = "world"
#ROS2 Spawn Entity
target_future = spawn_cli.call_async(self.spawn_request)
rclpy.spin_until_future_complete(self.node, target_future)
# Seed the environment
self.seed()
self.buffer_dist_rewards = []
self.buffer_tot_rewards = []
self.collided = 0
def getKey():
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def vels(speed, turn):
return "currently:\tspeed %s\tturn %s " % (speed, turn)
if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)
rclpy.init()
node = rclpy.create_node('teleop_keyboard')
pub = node.create_publisher(Twist, '/cmd_vel', 10)
speed = 0.1 #node.get_parameter('speed', 0.5)
turn = 0.1 #node.get_parameter('turn', 1.0)
x = 0
y = 0
z = 0
th = 0
status = 0
try:
print(msg)
print(vels(speed, turn))
while (1):
def main(args=None):
rclpy.init(args=args)
ralph_node = Ralph()
rclpy.spin(ralph_node)
ralph_node.destroy_node()
rclpy.shutdown()
def main(args=None):
rclpy.init(args=args)
node = CodeMapLocalizationSerial()
rclpy.spin(node)
def __init__(self):
rclpy.init()
super().__init__('state_publisher')
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, 'joint_states',
qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
degree = pi / 180.0
loop_rate = self.create_rate(30)
# robot state
tilt = 0.
tinc = degree
swivel = 0.
angle = 0.
height = 0.
hinc = 0.005
# message declarations
odom_trans = TransformStamped()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'chassis'
joint_state = JointState()
try:
while rclpy.ok():
rclpy.spin_once(self)
# update joint_state
now = self.get_clock().now()
joint_state.header.stamp = now.to_msg()
joint_state.name = ['swivel', 'tilt', 'periscope']
joint_state.position = [swivel, tilt, height]
# update transform
# (moving in a circle with radius=2)
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = cos(angle) * 2
odom_trans.transform.translation.y = sin(angle) * 2
odom_trans.transform.translation.z = 0.7
odom_trans.transform.rotation = \
euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw
# send the joint state and transform
self.joint_pub.publish(joint_state)
self.broadcaster.sendTransform(odom_trans)
# Create new robot state
tilt += tinc
if tilt < -0.5 or tilt > 0.0:
tinc *= -1
height += hinc
if height > 0.2 or height < 0.0:
hinc *= -1
swivel += degree
angle += degree / 4
# This will adjust as needed per iteration
loop_rate.sleep()
except KeyboardInterrupt:
pass
def sync_ros_properties(ctx: ContextWrapper):
"""
State that creates a ROS2-Node, registers all Ros2SubProperties and Ros2PubProperties in ROS2 and keeps them synced
"""
# check for ROS2 availability
if not ROS2_AVAILABLE:
logger.error(
"ROS2 is not available, therefore all ROS2-Properties "
"will be just normal properties without connection to ROS2!")
return Delete()
# get config stuff
node_name = ctx.conf(key=NODE_NAME_CONFIG_KEY)
if not node_name:
logger.error(
f"{NODE_NAME_CONFIG_KEY} is not set. Shutting down ravestate_ros2")
return Delete()
spin_frequency = ctx.conf(key=SPIN_FREQUENCY_CONFIG_KEY)
if spin_frequency is None or spin_frequency < 0:
logger.error(
f"{SPIN_FREQUENCY_CONFIG_KEY} is not set or less than 0. Shutting down ravestate_ros2"
)
return Delete()
if spin_frequency == 0:
spin_sleep_time = 0
else:
spin_sleep_time = 1 / spin_frequency
# init ROS
if not rclpy.ok():
rclpy.init()
node = rclpy.create_node(node_name)
global global_prop_set
# current_props: hash -> subscription/publisher
current_props: Dict = dict()
# ROS-Context Sync Loop
while not ctx.shutting_down():
# remove deleted props
removed_props = current_props.keys() - global_prop_set
for prop_hash in removed_props:
item = current_props[prop_hash]
if isinstance(item, rclpy.subscription.Subscription):
node.destroy_subscription(item)
elif isinstance(item, rclpy.publisher.Publisher):
node.destroy_publisher(item)
elif isinstance(item, rclpy.client.Client):
node.destroy_client(item)
current_props.pop(prop_hash)
# add new props
new_props = global_prop_set - current_props.keys()
for prop in new_props:
# register subscribers in ROS
if isinstance(prop, Ros2SubProperty):
# register in context
@receptor(ctx_wrap=ctx, write=prop.id())
def ros_to_ctx_callback(ctx, msg, prop_name: str):
ctx[prop_name] = msg
prop.ros_to_ctx_callback = ros_to_ctx_callback
prop.subscription = node.create_subscription(
prop.msg_type, prop.topic, prop.ros_subscription_callback)
current_props[prop.__hash__()] = prop.subscription
# register publishers in ROS
if isinstance(prop, Ros2PubProperty):
prop.publisher = node.create_publisher(prop.msg_type,
prop.topic)
current_props[prop.__hash__()] = prop.publisher
# register clients in ROS
if isinstance(prop, Ros2CallProperty):
prop.client = node.create_client(prop.service_type,
prop.service_name)
current_props[prop.__hash__()] = prop.client
# replace prop with hash in global_props
global_prop_set.remove(prop)
global_prop_set.add(prop.__hash__())
# spin once
rclpy.spin_once(node, timeout_sec=0)
time.sleep(spin_sleep_time)
node.destroy_node()
rclpy.shutdown()
print(f"\t↳ {Colors.BOLDCYAN}{s.message}{Colors.RESET}")
max_k_len = 0
for v in s.values:
if len(v.key) > max_k_len:
max_k_len = len(v.key)
for v in s.values:
print(f"\t {v.key:<{max_k_len+1}}= {v.value}")
print("-----")
def timer_cb():
print(f"\nDiagnostics timeout from [{frame_id}]\n")
timer.cancel()
rclpy.init(args=sys.argv)
node = Node("diag_listener", namespace=namespace)
if namespace == "/":
time.sleep(1)
nns = node.get_node_names_and_namespaces()
discovered_ns = {ns for (_, ns) in nns if ns != "/"}
if discovered_ns:
ns_selection = {
i + 1: ns
for (i, ns) in enumerate(sorted(discovered_ns))
}
node.get_logger().warn(
f"Namespace not specified, use '__ns:=/XXXX' argument for specific namespace"
)
print("Discovered namespaces:")
for i, ns in ns_selection.items():
y_ir = y_rgb * ir_h * 0.75 + 15
return [int(y_ir), int(x_ir)]
def ctof(deg_c):
return round(1.8 * deg_c + 32, 2)
if __name__ == "__main__":
DEMO_HEIGHT = 320
RGB_SHAPE = (1280, 720)
# RGB_SHAPE = (3264, 2464)
# Create detections publisher
rclpy.init(args=None)
node = rclpy.create_node("minimal_publisher")
publisher = node.create_publisher(String, "rgb_detections", 10)
irclient = HeatmapClient()
executor = MultiThreadedExecutor(num_threads=1)
executor.add_node(irclient)
from threading import Thread
t = Thread()
t.run = executor.spin
t.start()
# Set up inference
net = jetson.inference.detectNet("ssd-inception-v2", threshold=0.5)
def setUpClass(cls):
rclpy.init()
cls.node = rclpy.create_node('my_node', namespace='/my_ns')
