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_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 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 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, 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)
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 run(self):
self.node = rclpy.create_node('topic_monitor')
try:
run_topic_listening(self.node, self.topic_monitor, self.options)
except KeyboardInterrupt:
self.stop()
raise
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(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 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 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(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 _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 get_state(lifecycle_node):
node = rclpy.create_node('lc_client_py')
cli = node.create_client(GetState, lifecycle_node + '__get_state')
req = GetState.Request()
cli.call(req)
cli.wait_for_future()
print('%s is in state %s(%u)'
% (lifecycle_node, cli.response.current_state.label, cli.response.current_state.id))
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 get_available_states(lifecycle_node):
node = rclpy.create_node('lc_client_py')
cli = node.create_client(GetAvailableStates, lifecycle_node + '__get_available_states')
req = GetAvailableStates.Request()
cli.call(req)
cli.wait_for_future()
print('%s has %u available states' % (lifecycle_node, len(cli.response.available_states)))
for state in cli.response.available_states:
print('id: %u\tlabel: %s' % (state.id, state.label))
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 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_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):
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_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 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):
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):
if args is None:
args = sys.argv
rclpy.init(args)
# TODO(wjwwood): move this import back to the top of the file when
# it is possible to import the messages before rclpy.init().
from std_msgs.msg import String
node = rclpy.create_node('listener')
sub = node.create_subscription(String, 'chatter', chatter_callback, qos_profile_default)
assert sub # prevent unused warning
while rclpy.ok():
rclpy.spin_once(node)
def get_available_transitions(lifecycle_node):
node = rclpy.create_node('lc_client_py')
cli = node.create_client(
GetAvailableTransitions, lifecycle_node + '__get_available_transitions')
req = GetAvailableTransitions.Request()
cli.call(req)
cli.wait_for_future()
print('%s has %u available transitions'
% (lifecycle_node, len(cli.response.available_transitions)))
for transition in cli.response.available_transitions:
print('Transition id: %u\tlabel: %s'
% (transition.transition.id, transition.transition.label))
print('\tStart id: %u\tlabel: %s'
% (transition.start_state.id, transition.start_state.label))
print('\tGoal id: %u\tlabel: %s'
% (transition.goal_state.id, transition.goal_state.label))
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('minimal_client')
# Node's default callback group is mutually exclusive. This would prevent the client response
# from being processed until the timer callback finished, but the timer callback in this
# example is waiting for the client response
cb_group = ReentrantCallbackGroup()
cli = node.create_client(AddTwoInts, 'add_two_ints', callback_group=cb_group)
did_run = False
did_get_result = False
async def call_service():
nonlocal cli, node, did_run, did_get_result
did_run = True
try:
req = AddTwoInts.Request()
req.a = 41
req.b = 1
future = cli.call_async(req)
result = await future
if result is not None:
node.get_logger().info(
'Result of add_two_ints: for %d + %d = %d' %
(req.a, req.b, result.sum))
else:
node.get_logger().info('Service call failed %r' % (future.exception(),))
finally:
did_get_result = True
while not cli.wait_for_service(timeout_sec=1.0):
node.get_logger().info('service not available, waiting again...')
timer = node.create_timer(0.5, call_service, callback_group=cb_group)
while rclpy.ok() and not did_run:
rclpy.spin_once(node)
if did_run:
# call timer callback only once
timer.cancel()
while rclpy.ok() and not did_get_result:
rclpy.spin_once(node)
node.destroy_node()
rclpy.shutdown()
def test_create_node_with_relative_namespace(self):
node_name = 'create_node_test'
namespace = 'ns'
node = rclpy.create_node(node_name, namespace=namespace)
self.assertEqual('/ns', node.get_namespace())
def main(args=None):
rclpy.init(args=args)
node = rclpy.create_node('keyboard_input3')
publisher = node.create_publisher(Twist, 'turtle1/cmd_vel', 10)
def get_keys(): #gets keyboard input
key = ''
k = ord(getch.getch()) #converts keypress to ord value
if (k == 119):
key = 'up' #up
elif (k == 115):
key = 'down' #down
elif (k == 97):
key = 'left' #left
elif (k == 100):
key = 'right' #right
elif (k == 113):
key = 'left_fwd' #turn left and forward
elif (k == 101):
key = 'right_fwd' #turn right and forward
elif (k == 114):
key = 'accel' #accelerate
elif (k == 116):
key = 'deccel' #deccelerate
elif (k == 27):
sys.exit("Exited Progam")
else:
key = ''
#rospy.loginfo(str(key)) #write val to terminal
return key
def velocityOut(speed, turn): #for printing/getting speed & turn angle
return "speed & turn angle: \tspeed %s\tturn %s " % (speed, turn)
def keyboard_input():
speed = 0.5 #default speed val
turn = 1.0 #default turn val
x = 0
y = 0
z = 0
th = 0
status = 0
while (1):
input = get_keys()
if (input == 'up'): #up
x = 1
y = 0
z = 0
th = 0
elif (input == 'down'): #down
x = -1
y = 0
z = 0
th = 0
elif (input == 'left'): #left
x = 0
y = 0
z = 0
th = 1
elif (input == 'right'): #right
x = 0
y = 0
z = 0
th = -1
elif (input == 'left_fwd'): #left + forward
x = 1
y = 0
z = 0
th = 1
elif (input == 'right_fwd'): #right + forward
x = 1
y = 0
z = 0
th = -1
elif (input == 'accel'): #accelerate by 10%
x = 0
y = 0
z = 0
th = 0
speed += 0.1
elif (input == 'deccel'): #deccelerate by 10%
x = 0
y = 0
z = 0
th = 0
speed -= 0.1
else:
x = 0
y = 0
z = 0
th = 0
twist = Twist()
twist.linear.x = x * speed
twist.linear.y = y * speed
twist.linear.z = z * speed
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = th * turn
publisher.publish(twist)
print(velocityOut(speed,
twist.angular.z)) #write speed/angle to terminal
print(directionInfo)
keyboard_input()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
from time import sleep
import rclpy
from std_msgs.msg import String
from CopyNet import predict_continuous
from DataPreProcessing import frame_parse, json_lint, assemble_graph, FIFOBuffer
# Press the green button in the gutter to run the script.
if __name__ == '__main__':
nl_input = "go to the door and turn left"
parsed = frame_parse(nl_input)
rclpy.init()
node = rclpy.create_node('semantic_parser')
pub = node.create_publisher(String, 'talker', 10)
msg = String()
while rclpy.ok():
decoded_output = []
for x in parsed:
output = predict_continuous(x)
linted_output = json_lint(output)
decoded_output.append(linted_output)
graph = assemble_graph(decoded_output)
msg.data = str(graph)
pub.publish(msg)
sleep(0.5)
def __init__(self):
"""
Initialize the MARA environemnt
"""
# Manage command line args
args = ut_generic.getParserArgsRobot().parse_args()
self.gzclient = args.gzclient
self.realSpeed = args.realSpeed
# self.realSpeed = True
self.debug = args.debug
self.multiInstance = args.multiInstance
self.port = args.port
# Set the path of the corresponding URDF file
if self.realSpeed:
urdf = "biped.urdf"
self.urdfPath = get_prefix_path(
"lobot_description"
) + "/share/lobot_description/robots/" + urdf
else:
print("Non real speed not yet supported. Use real speed instead. ")
# TODO: Include launch logic here, refer to code from the .launch.py files
# Note that after including the launch logic the code will no longer be debuggable due to multi process stuff
# Create the node after the new ROS_DOMAIN_ID is set in generate_launch_description()
rclpy.init()
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
#############################
EE_POINTS = np.asmatrix([[0, 0, 0]])
EE_VELOCITIES = np.asmatrix([[0, 0, 0]])
# # Topics for the robot publisher and subscriber.
JOINT_PUBLISHER = '/lobot_arm/control'
# Get Joint names from the parameter server
get_joints_client = self.node.create_client(
GetAllJoints,
"/GetAllControlJoints",
qos_profile=qos_profile_services_default)
req = GetAllJoints.Request()
req.robot = "lobot_arm"
while not get_joints_client.wait_for_service(timeout_sec=3.0):
self.node.get_logger().info(
'service not available, waiting again...')
future = get_joints_client.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
if future.result() is not None:
joint_names = future.result().joints
self.node.get_logger().info('Number of joints: %d' %
(len(joint_names)))
else:
self.node.get_logger().info('Service call failed %r' %
(future.exception(), ))
JOINT_ORDER = joint_names
INITIAL_JOINTS = np.full((len(joint_names)), 0.0).tolist()
reset_condition = {
'initial_positions': INITIAL_JOINTS,
'initial_velocities': []
}
#############################
m_jointOrder = copy.deepcopy(JOINT_ORDER)
# Initialize target end effector position
self.environment = {
'jointOrder': m_jointOrder,
'reset_conditions': reset_condition,
'tree_path': self.urdfPath,
'end_effector_points': EE_POINTS,
}
# Subscribe to the appropriate topics, taking into account the particular robot
self._pub = self.node.create_publisher(
JointControl, JOINT_PUBLISHER, qos_profile=qos_profile_sensor_data)
self._sub = self.node.create_subscription(JointState, "/joint_states",
self.observation_callback,
qos_profile_sensor_data)
# TODO: Make the clock node run on a separate thread so weird issues like outdated clock can stop happening
self.lock = threading.Lock()
self.clock_node = rclpy.create_node(self.__class__.__name__ + "_clock")
self._sub_clock = self.clock_node.create_subscription(
RosClock,
'/clock',
self.clock_callback,
qos_profile=qos_profile_sensor_data)
self.exec = rclpy.executors.MultiThreadedExecutor()
self.exec.add_node(self.clock_node)
t1 = threading.Thread(target=self.spinClockNode, daemon=True)
t1.start()
# self._imu_sub = self.node.create_subscription(JointState, "/lobot_IMU_controller/out", self.imu_callback, qos_profile_sensor_data)
# self._sub = self.node.create_subscription(JointTrajectoryControllerState, JOINT_SUBSCRIBER, self.observation_callback, qos_profile=qos_profile_sensor_data)
self._reset_sim = self.node.create_client(Empty, '/reset_simulation')
self._physics_pauser = self.node.create_client(Empty, '/pause_physics')
self._robot_resetter = self.node.create_client(Empty,
'/lobot_arm/reset')
self._physics_unpauser = self.node.create_client(
Empty, '/unpause_physics')
self.delete_entity = self.node.create_client(DeleteEntity,
'/delete_entity')
self.numJoints = len(JOINT_ORDER)
# Initialize a KDL Jacobian solver from the chain.
# self.jacSolver = ChainJntToJacSolver(self.mara_chain)
# Observable dimensions, each joint has 2 (joint position + joint velocity), the IMU gives 6
self.obs_dim = self.numJoints * 2 + 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) * 0.4
high = np.pi * np.ones(self.numJoints) * 0.4
self.action_space = spaces.Box(low, high)
high = np.inf * np.ones(self.obs_dim)
low = -high
self.observation_space = spaces.Box(low, high)
self.seed()
self.buffer_dist_rewards = []
self.buffer_tot_rewards = []
self.collided = 0
# Set the time source
self._sim_time = 0
self._sim_time_msg = builtin_interfaces.msg.Time()
def robot_description_client(init_node):
return RobotDescriptionClient(
rclpy.create_node('robot_description_client'))
def test_create_node_with_namespace(self):
node_name = 'create_node_test'
namespace = '/ns'
rclpy.create_node(node_name, namespace=namespace)
def test_initially_no_executor(self):
node = rclpy.create_node('my_node', context=self.context)
try:
assert node.executor is None
finally:
node.destroy_node()
def main():
# A flow of typical ROS program
# 1. Initialization
rclpy.init()
# 2. Create one or more nodes
node = rclpy.create_node('autopilot')
cb_group = rclpy.callback_groups.ReentrantCallbackGroup()
client = node.create_client(GetPosition,
'get_position',
callback_group=cb_group)
ready = False
while not ready:
node.get_logger().info('waiting for the service availablity.')
ready = client.wait_for_service(timeout_sec=1.0)
current_position = GetPosition.Response()
async def execute_callback(goal_handle):
"""Generate autopilot sequence"""
nonlocal current_position
# align x-axis position
goal_head = 'east' if current_position.x < goal_handle.request.goal_x else 'west'
feedback = AutoPilot.Feedback()
feedback.partial_cmds = []
while current_position.head != goal_head:
feedback.partial_cmds.append(cmd['turn_left'])
node.get_logger().info('auto steering: turn left')
goal_handle.publish_feedback(feedback)
if goal_handle.is_cancel_requested:
node.get_logger().info('Goal canceled')
goal_handle.canceled()
return AutoPilot.Result()
time.sleep(1)
req = GetPosition.Request()
current_position = await client.call_async(req)
while current_position.x != goal_handle.request.goal_x:
feedback.partial_cmds.append(cmd['move_forward'])
node.get_logger().info('auto steering: move forward')
goal_handle.publish_feedback(feedback)
if goal_handle.is_cancel_requested:
node.get_logger().info('Goal canceled')
goal_handle.canceled()
return AutoPilot.Result()
time.sleep(1)
req = GetPosition.Request()
current_position = await client.call_async(req)
goal_head = 'north' if current_position.y < goal_handle.request.goal_y else 'south'
while current_position.head != goal_head:
feedback.partial_cmds.append(cmd['turn_left'])
node.get_logger().info('auto steering: turn left')
goal_handle.publish_feedback(feedback)
if goal_handle.is_cancel_requested:
node.get_logger().info('Goal canceled')
goal_handle.canceled()
return AutoPilot.Result()
time.sleep(1)
req = GetPosition.Request()
current_position = await client.call_async(req)
while current_position.y != goal_handle.request.goal_y:
feedback.partial_cmds.append(cmd['move_forward'])
node.get_logger().info('auto steering: move forward')
goal_handle.publish_feedback(feedback)
if goal_handle.is_cancel_requested:
node.get_logger().info('Goal canceled')
goal_handle.canceled()
return AutoPilot.Result()
time.sleep(1)
req = GetPosition.Request()
current_position = await client.call_async(req)
result = AutoPilot.Result()
result.cmds = feedback.partial_cmds
goal_handle.succeed()
return result
async def goal_callback(goal_request):
"""Accepts or rejects a client request to begin an action."""
nonlocal current_position
# request GetPosition service
req = GetPosition.Request()
future = client.call_async(req)
try:
current_position = await future
except Exception as e:
node.get_logger().info('Destination goal failed: {}'.format(e))
return rclpy.action.GoalResponse.REJECT
node.get_logger().info('Destination goal accepted!')
return rclpy.action.GoalResponse.ACCEPT
def cancel_callback(goal_handle):
"""Accepts or rejects a client request to cancel an action."""
node.get_logger().info('Destination goal canceled!')
return rclpy.action.CancelResponse.ACCEPT
# 3. Process node callbacks
action_server = rclpy.action.ActionServer(
node,
AutoPilot,
'autopilot',
execute_callback=execute_callback,
callback_group=cb_group,
goal_callback=goal_callback,
cancel_callback=cancel_callback)
node.get_logger().info('Action server in action!')
rclpy.spin(node, executor=rclpy.executors.MultiThreadedExecutor())
# 4. Shutdown
action_server.destroy()
node.destroy_node()
rclpy.shutdown()
def setUpClass(cls):
rclpy.init()
cls.node = rclpy.create_node('TestClient')
import rclpy
from rclpy.qos import qos_profile_sensor_data
from hrim_actuator_rotaryservo_msgs.msg import StateRotaryServo
# Function that will be called once a message is published to the topic we are subscribed
def minimal_callback(msg):
print('Position:', str(msg.position))
# -------- #
rclpy.init(args=None)
# Create Node with name "mara_minimal_subscriber"
node = rclpy.create_node('mara_minimal_subscriber')
# Subscribe to topic "/hrim_actuation_servomotor_000000000001/state_axis1" and link it to "minimal_callback" function
node.create_subscription(StateRotaryServo,
'/hrim_actuator_rotaryservo_000000000001/state_axis1',
minimal_callback,
qos_profile=qos_profile_sensor_data
) # QoS profile for reading (joint) sensors
# Spin listening to all subscribed topics
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'send_waypoint_file',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Send a waypoint file, namespace=%s' %
node.get_namespace())
if node.has_parameter('filename'):
filename = node.get_parameter(
'filename').get_parameter_value().string_value
else:
raise RuntimeError('No filename found')
# Important...ensure the clock has been updated when using sim time
while node.get_clock().now() == rclpy.time.Time(
clock_type=node.get_clock().clock_type):
rclpy.spin_once(node)
# If no start time is provided: start *now*.
start_time = time_in_float_sec(node.get_clock().now())
start_now = True
if node.has_parameter('start_time'):
start_time = node.get_parameter('start_time').value
if start_time < 0.0:
node.get_logger().warn('Negative start time, setting it to 0.0')
start_time = 0.0
start_now = True
else:
start_now = False
node.get_logger().info('Start time=%.2f s' % start_time)
interpolator = node.get_parameter_or(
'interpolator', 'lipb').get_parameter_value().string_value
srv_name = 'init_waypoints_from_file'
init_wp = node.create_client(InitWaypointsFromFile, srv_name)
ready = init_wp.wait_for_service(timeout_sec=30)
if not ready:
raise RuntimeError('Service not available! Closing node...')
(sec, nsec) = float_sec_to_int_sec_nano(start_time)
req = InitWaypointsFromFile.Request()
req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg()
req.start_now = start_now
req.filename = String(data=filename)
req.interpolator = String(data=interpolator)
future = init_wp.call_async(req)
rclpy.spin_until_future_complete(node, future)
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name +
' failed, error=' + repr(e))
else:
node.get_logger().info('Waypoints file successfully received, '
'filename=%s' % filename)
node.destroy_node()
def setUp(self):
# Create a ROS node for tests
self.node = rclpy.create_node(
'test_thrusters_allocator',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True)
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_thrusters_states',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info(
'Set the state of thrusters for vehicle, namespace=' +
node.get_namespace())
thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
thread.start()
# Make sure the clock has been updated when using sim time
while node.get_clock().now() == rclpy.time.Time(
clock_type=node.get_clock().clock_type):
pass
starting_time = 0.0
if node.has_parameter('starting_time'):
starting_time = node.get_parameter('starting_time').value
if starting_time < 0:
starting_time = time_in_float_sec(node.get_clock().now())
node.get_logger().info('Starting time={} s'.format(starting_time))
duration = 0.0
if node.has_parameter('duration'):
duration = node.get_parameter('duration').value
if duration == 0.0:
raise RuntimeError('Duration not set, leaving node...')
node.get_logger().info('Duration [s]=%s' %
('Inf.' if duration < 0 else str(duration)))
if node.has_parameter('is_on'):
is_on = bool(
node.get_parameter('is_on').get_parameter_value().bool_value)
else:
raise RuntimeError('is_on state flag not provided')
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 state of thruster #{} as {}'.format(
thruster_id, 'ON' if is_on else 'OFF'))
vehicle_name = node.get_namespace().replace('/', '')
srv_name = build_service_name(vehicle_name, thruster_id,
'set_thruster_state')
try:
set_state = node.create_client(SetThrusterState, srv_name)
except Exception as e:
raise RuntimeError('Service call failed, error=' + str(e))
if not set_state.wait_for_service(timeout_sec=2):
raise RuntimeError('Service %s not available! Closing node...' %
(srv_name))
FREQ = 100
rate = node.create_rate(FREQ)
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()
# First request
req = SetThrusterState.Request()
req.on = is_on
future = set_state.call_async(req)
# NB : spining is done from another thread
while not future.done():
pass
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name +
' failed, error=' + repr(e))
else:
if response.success:
node.get_logger().info('Time={} s'.format(
time_in_float_sec(node.get_clock().now())))
node.get_logger().info('Current state of thruster #{}={}'.format(
thruster_id, 'ON' if is_on else 'OFF'))
else:
node.get_logger().error('Service call ' + srv_name +
' returned a "failed" value')
# Returning to previous state
if duration > 0:
while time_in_float_sec(
node.get_clock().now()) < starting_time + duration:
if 1.0 / FREQ < starting_time + duration:
rate.sleep()
req.on = not is_on
success = set_state.call(req)
while not future.done():
pass
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name +
' failed, error=' + str(e))
else:
if response.success:
node.get_logger().info('Time={} s'.format(
time_in_float_sec(node.get_clock().now())))
node.get_logger().info(
'Returning to previous state of thruster #{}={}'.format(
thruster_id, 'ON' if not is_on else 'OFF'))
else:
node.get_logger().error('Service call ' + srv_name +
' returned a "failed" value')
node.get_logger().info('Leaving node...')
node.destroy_node()
rclpy.shutdown()
thread.join()
def main():
settings = termios.tcgetattr(sys.stdin)
rclpy.init()
qos = QoSProfile(depth=10)
node = rclpy.create_node('teleop_keyboard')
pub = node.create_publisher(Twist, 'cmd_vel', qos)
idx = 0
status = 0
target_linear_velocity = 0.0
target_angular_velocity = 0.0
control_linear_velocity = 0.0
control_angular_velocity = 0.0
try:
print(msg)
while (1):
key = get_key(settings, idx)
print(key)
idx = idx + 1
if key == 'w':
#target_linear_velocity =\
# check_linear_limit_velocity(target_linear_velocity + LIN_VEL_STEP_SIZE)
target_linear_velocity = target_linear_velocity + 0.02
status = status + 1
print_vels(target_linear_velocity, target_angular_velocity)
elif key == 'x':
#target_linear_velocity =\
# check_linear_limit_velocity(target_linear_velocity - LIN_VEL_STEP_SIZE)
target_linear_velocity = target_linear_velocity - 0.02
status = status + 1
print_vels(target_linear_velocity, target_angular_velocity)
elif key == 'a':
target_angular_velocity =\
check_angular_limit_velocity(target_angular_velocity + ANG_VEL_STEP_SIZE)
status = status + 1
print_vels(target_linear_velocity, target_angular_velocity)
elif key == 'd':
target_angular_velocity =\
check_angular_limit_velocity(target_angular_velocity - ANG_VEL_STEP_SIZE)
status = status + 1
print_vels(target_linear_velocity, target_angular_velocity)
elif key == ' ' or key == 's':
target_linear_velocity = 0.0
control_linear_velocity = 0.0
target_angular_velocity = 0.0
control_angular_velocity = 0.0
print_vels(target_linear_velocity, target_angular_velocity)
else:
if (key == '\x03'):
break
if idx == 14:
idx = 0
if status == 20:
print(msg)
status = 0
twist = Twist()
control_linear_velocity = make_simple_profile(
control_linear_velocity, target_linear_velocity,
(LIN_VEL_STEP_SIZE / 2.0))
twist.linear.x = control_linear_velocity
twist.linear.y = 0.0
twist.linear.z = 0.0
control_angular_velocity = make_simple_profile(
control_angular_velocity, target_angular_velocity,
(ANG_VEL_STEP_SIZE / 2.0))
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = control_angular_velocity
pub.publish(twist)
except Exception as e:
print(e)
finally:
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node(
'set_body_wrench',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Apply programmed perturbation to vehicle ' +
node.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)
#starting time_clock = node.get_clock().now() + rclpy.duration.Duration(nanoseconds = starting_time * 1e9)
node.get_logger().info('Starting time in = {} s'.format(starting_time))
duration = 0.0
if node.has_parameter('duration'):
duration = float(node.get_parameter('duration').value)
#compare to eps ?
if duration <= 0.0:
node.get_logger().info('Duration not set, leaving node...')
sys.exit(-1)
node.get_logger().info('Duration [s]=' +
('Inf.' if duration < 0 else str(duration)))
force = [0.0, 0.0, 0.0]
if node.has_parameter('force'):
force = node.get_parameter('force').value
#print(force)
if len(force) != 3:
raise RuntimeError('Invalid force vector')
#Ensure type is float
force = [float(elem) for elem in force]
node.get_logger().info('Force [N]=' + str(force))
torque = [0.0, 0.0, 0.0]
if node.has_parameter('torque'):
torque = node.get_parameter('torque').value
if len(torque) != 3:
raise RuntimeError('Invalid torque vector')
#Ensure type is float
torque = [float(elem) for elem in torque]
node.get_logger().info('Torque [N]=' + str(torque))
service_name = '/gazebo/apply_link_wrench'
try:
apply_wrench = node.create_client(ApplyLinkWrench, service_name)
except Exception as e:
node.get_logger().error('Creation of service ' + service_name +
' failed, error=' + str(e))
sys.exit(-1)
try:
ready = apply_wrench.wait_for_service(timeout_sec=10)
if not ready:
raise RuntimeError('')
except:
node.get_logger().error('Service ' + service_name +
' not available! Closing node...')
sys.exit(-1)
ns = node.get_namespace().replace('/', '')
body_name = '%s/base_link' % ns
FREQ = 100
rate = node.create_rate(
FREQ
) #, rclpy.clock.Clock(clock_type=rclpy.clock.ClockType.STEADY_TIME))
thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
thread.start()
while time_in_float_sec(node.get_clock().now()) < starting_time:
#rclpy.spin_once(node)
#Just a guard for really short timeouts
if 1.0 / FREQ < starting_time:
rate.sleep()
# time.sleep(starting_time)
# if starting_time >= 0:
# rate = node.create_rate(100)
# while node.get_clock().now() < starting_time_clock:
# rate.sleep()
apw = ApplyLinkWrench.Request()
apw.link_name = body_name
apw.reference_frame = 'world'
apw.reference_point = Point(x=0.0, y=0.0, z=0.0)
apw.wrench = Wrench()
apw.wrench.force = Vector3(x=force[0], y=force[1], z=force[2])
apw.wrench.torque = Vector3(x=torque[0], y=torque[1], z=torque[2])
apw.start_time = node.get_clock().now().to_msg()
apw.duration = rclpy.time.Duration(seconds=duration).to_msg()
future = apply_wrench.call_async(apw)
#NB : spining is done from another thread
while rclpy.ok():
if future.done():
try:
response = future.result()
except Exception as e:
node.get_logger().info('Could not apply link wrench %r' %
(e, ))
else:
node.get_logger().info('Link wrench perturbation applied!')
node.get_logger().info('\tFrame: ' + body_name)
node.get_logger().info('\tDuration [s]: ' + str(duration))
node.get_logger().info('\tForce [N]: ' + str(force))
node.get_logger().info('\tTorque [Nm]: ' + str(torque))
break
res.response = '\n'.join(results)
return res
# global
clients = []
cfg = ros2_config.get()
nodename = cfg[0]['name']
for i in cfg:
clients.append(i['name'])
rclpy.init()
node = rclpy.create_node(nodename+"_master")
event_loop = asyncio.get_event_loop()
print("Starting master services:")
#print(" master/signup")
#s1 = node.create_service(StrReqStrRes, 'master/signup', signup_callback)
print(" master/cmd_all")
s2 = node.create_service(StrReqStrRes, 'master/cmd_all', cmdall_callback)
rclpy.spin(node)
# Destroy the node explicitly
# (optional - Done automatically when node is garbage collected)
node.destroy_node()
rclpy.shutdown()
def setUpClass(cls):
cls.context = rclpy.context.Context()
rclpy.init(context=cls.context)
cls.node = rclpy.create_node(TEST_NODE,
namespace=TEST_NAMESPACE,
context=cls.context)
node.destroy_node()
# ==========================================
# 返回页面
# ==========================================
@app.route('/')
def index():
return render_template('index.html', async_mode=socketio.async_mode)
# ============================================================================================================================
# ===================================================================== others ============================================
# ==========================================
# init node
# ==========================================
rclpy.init()
node = rclpy.create_node('referee_web')
# ==========================================
# init namespace class
# ==========================================
socketio.on_namespace(RefereeSocketHandler('/'))
if __name__ == '__main__':
if len(sys.argv) == 3:
robot_name = str(sys.argv[1])
port = int(sys.argv[2])
socketio.run(app, host='0.0.0.0', port=2350)
def main():
rclpy.init()
sim_time_param = is_sim_time()
# Compared to ROS 1 version, the node name was changed
node = rclpy.create_node(
'start_helical_trajectory',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
node.get_logger().info('Starting the helical trajectory creator')
# Ensure the clock has been updated when using sim time
while node.get_clock().now() == rclpy.time.Time(
clock_type=node.get_clock().clock_type):
rclpy.spin_once(node)
# If no start time is provided: start *now*.
start_time = time_in_float_sec(node.get_clock().now())
start_now = False
if node.has_parameter('start_time'):
start_time = node.get_parameter('start_time').value
if start_time < 0.0:
node.get_logger().warn('Negative start time, setting it to 0.0')
start_time = 0.0
start_now = True
else:
start_now = True
param_labels = [
'radius', 'center', 'n_points', 'heading_offset', 'duration',
'n_turns', 'delta_z', 'max_forward_speed'
]
params = dict()
for label in param_labels:
if not node.has_parameter(label):
node.get_logger().error(
'{} must be provided for the trajectory generation!'.format(
label))
sys.exit(-1)
params[label] = node.get_parameter(label).value
if len(params['center']) != 3:
raise RuntimeError(
'Center of circle must have 3 components (x, y, z):' +
str(params['center']))
if params['n_points'] <= 2:
raise RuntimeError('Number of points must be at least 2' +
str(params['n_points']))
if params['max_forward_speed'] <= 0:
raise RuntimeError('Velocity limit must be positive' +
str(params['max_forward_speed']))
srv_name = 'start_helical_trajectory'
traj_gen = node.create_client(InitHelicalTrajectory,
'start_helical_trajectory')
if not traj_gen.wait_for_service(timeout_sec=20):
raise RuntimeError('Service %s not available! Closing node...' %
(traj_gen.srv_name))
node.get_logger().info(
'Generating trajectory that starts at t={} s'.format(start_time))
# Convert the time value
(sec, nsec) = float_sec_to_int_sec_nano(start_time)
req = InitHelicalTrajectory.Request()
req.start_time = rclpy.time.Time(seconds=sec, nanoseconds=nsec).to_msg()
req.start_now = start_now
req.radius = params['radius']
req.center = Point(params['center'][0], params['center'][1],
params['center'][2])
req.is_clockwise = False
req.angle_offset = 0.0
req.n_points = params['n_points']
req.heading_offset = params['heading_offset'] * pi / 180
req.max_forward_speed = params['max_forward_speed']
req.duration = params['duration']
req.n_turns = params['n_turns']
req.delta_z = params['delta_z']
future = traj_gen.call_async(req)
rclpy.spin_until_future_complete(node, future)
try:
response = future.result()
except Exception as e:
node.get_logger().error('Service call ' + srv_name +
' failed, error=' + str(e))
else:
node.get_logger().info('Trajectory successfully generated!')
node.destroy_node()
def test_create_node(self):
node_name = 'create_node_test'
rclpy.create_node(node_name, context=self.context).destroy_node()
def __init__(self, rsw_id, params):
#（__init__における handler の組み込み場所）
self.node = rclpy.create_node(node_name)
self.node.declare_parameter('~node_name')
self.node.declare_parameter('~rsw_id')
self.node.declare_parameter('~use_axis')
self.name = self.node.get_parameter('~node_name').get_parameter_value().string_value
self.rsw_id = self.node.get_parameter('~rsw_id').get_parameter_value().string_value
self._use_axis = self.node.get_parameter('~use_axis').get_parameter_value().string_value
self.params = {}
for ax in self._use_axis:
params[ax] = {}
#params[ax]['mode'] = rospy.get_param('~{ax}_mode'.format(**locals()), default_mode)
#params[ax]['pulse_conf'] = [eval(rospy.get_param('~{ax}_pulse_conf'.format(**locals()), default_pulse_conf))] #evalとは？？？
self.node.declare_parameter('~{ax}_mode')
self.node.declare_parameter('~{ax}_pulse_conf')
self.params[ax]['mode'] = self.node.get_parameter('~{ax}_mode').get_parameter_value().string_value
self.params[ax]['pulse_conf'] = self.node.get_parameter('~{ax}_pulse_conf').get_parameter_value().string_value
self.mp = {}
# mp['clock'] = rospy.get_param('~{ax}_clock'.format(**locals()), default_clock)
# mp['acc_mode'] = rospy.get_param('~{ax}_acc_mode'.format(**locals()), default_acc_mode)
# mp['low_speed'] = rospy.get_param('~{ax}_low_speed'.format(**locals()), default_low_speed)
# mp['speed'] = rospy.get_param('~{ax}_speed'.format(**locals()), default_speed)
# mp['acc'] = rospy.get_param('~{ax}_acc'.format(**locals()), default_acc)
# mp['dec'] = rospy.get_param('~{ax}_dec'.format(**locals()), default_dec)
# mp['step'] = rospy.get_param('~{ax}_step'.format(**locals()), default_step)
self.node.declare_parameter('~{ax}_clock')
self.node.declare_parameter('~{ax}_acc_mode')
self.node.declare_parameter('~{ax}_low_speed')
self.node.declare_parameter('~{ax}_speed')
self.node.declare_parameter('~{ax}_acc')
self.node.declare_parameter('~{ax}_dec')
self.node.declare_parameter('~{ax}_step')
self.mp['clock'] = self.node.get_parameter('~{ax}_clock').get_parameter_value().string_value #mpにはselfはいらない？？ paramsに格納するから
self.mp['acc_mode'] = self.node.get_parameter('~{ax}_acc_mode').get_parameter_value().string_value
self.mp['low_speed'] = self.node.get_parameter('~{ax}_low_speed').get_parameter_value().string_value
self.mp['speed'] = self.node.get_parameter('~{ax}_speed').get_parameter_value().string_value
self.mp['acc'] = self.node.get_parameter('~{ax}_acc').get_parameter_value().string_value
self.mp['dec'] = self.node.get_parameter('~{ax}_dec').get_parameter_value().string_value
self.mp['step'] = self.node.get_parameter('~{ax}_step').get_parameter_value().string_value
self.params[ax]['motion'] = mp
continue
#元driver
self.func_queue = queue.Queue()
self.pub = {}
self.use_axis = ''.join([ax for ax in self.params])
self.mode = [self.params[ax]['mode'] for ax in self.use_axis]
self.motion = {ax: self.params[ax]['motion'] for ax in self.use_axis}
#元handler
#use axis
self.current_speed = {ax: 0 for ax in self.use_axis}
self.current_step = {ax: 0 for ax in self.use_axis}
self.current_moving = {ax: 0 for ax in self.use_axis}
self.last_direction = {ax: 0 for ax in self.use_axis}
self.do_status = [0, 0, 0, 0]
self.move_mode = {ax: p['mode'] for ax, p in self.params.items()}
self.default_speed = {ax: p['motion']['speed'] for ax, p in self.params.items()}
self.low_speed = {ax: p['motion']['low_speed'] for ax, p in self.params.items()}
self.mot = pyinterface.open(7415, self.rsw_id)
[self.mot.set_pulse_out(ax, 'method', self.params[ax]['pulse_conf']) for ax in self.use_axis]
self.mot.set_motion(self.use_axis, self.mode, self.motion)
#元handler 命令値を受け取る
base = '/pyinterface/pci7415/rsw{self.rsw_id}'.format(**locals())
for ax in self.use_axis:
b = '{base}/{ax}/'.format(**locals())
# rospy.Subscriber(b+'step_cmd', std_msgs.msg.Int64, self.set_step, callback_args=ax)
self.node.create_subscription(std_msgs.msg.Int64, b+'step_cmd', partial(self.set_step, ax))
# rospy.Subscriber(b+'speed_cmd', std_msgs.msg.Float64, self.set_speed, callback_args=ax)
self.node.create_subscription(std_msgs.msg.Int64, b+'speed_cmd', partial(self.set_speed, ax))
for do_num in range(1,5):
self.node.create_subscription(std_msgs.msg.Int64, '{}/output_do{}_cmd'.format(base, do_num), partial(self.set_do, do_num))
#rospy.Subscriber('{}/output_do{}_cmd'.format(base, do_num), std_msgs.msg.Int64, self.set_do, callback_args=do_num)
# loop start 元driver
# self.th = threading.Thread(target=self.loop)
# self.th.setDaemon(True)
# self.th.start()
self.node.create_timer(1e-5,self.loop)
return
def test_create_node_with_empty_namespace(self):
node_name = 'create_node_test'
namespace = ''
node = rclpy.create_node(node_name, namespace=namespace)
self.assertEqual('/', node.get_namespace())
def main():
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument(
'data_name',
nargs='?',
default='topic1',
help='Name of the data (must comply with ROS topic rules)')
parser.add_argument('--best-effort',
action='store_true',
default=False,
help="Set QoS reliability option to 'best effort'")
parser.add_argument('--transient-local',
action='store_true',
default=False,
help="Set QoS durability option to 'transient local'")
parser.add_argument('--depth',
type=int,
default=default_depth,
help='Size of the QoS history depth')
parser.add_argument(
'--keep-all',
action='store_true',
default=False,
help=
"Set QoS history option to 'keep all' (unlimited depth, subject to resource limits)"
)
parser.add_argument('--payload-size',
type=int,
default=0,
help='Size of data payload to send')
parser.add_argument('--period',
type=float,
default=0.5,
help='Time in seconds between messages')
parser.add_argument(
'--end-after',
type=int,
help='Script will exit after publishing this many messages')
args = parser.parse_args()
reliability_suffix = '_best_effort' if args.best_effort else ''
topic_name = '{0}_data{1}'.format(args.data_name, reliability_suffix)
rclpy.init()
node = rclpy.create_node('%s_pub' % topic_name)
node_logger = node.get_logger()
qos_profile = QoSProfile(depth=args.depth)
if args.best_effort:
node_logger.info('Reliability: best effort')
qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
else:
node_logger.info('Reliability: reliable')
qos_profile.reliability = QoSReliabilityPolicy.RELIABLE
if args.keep_all:
node_logger.info('History: keep all')
qos_profile.history = QoSHistoryPolicy.KEEP_ALL
else:
node_logger.info('History: keep last')
qos_profile.history = QoSHistoryPolicy.KEEP_LAST
node_logger.info('Depth: {0}'.format(args.depth))
if args.transient_local:
node_logger.info('Durability: transient local')
qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
else:
node_logger.info('Durability: volatile')
qos_profile.durability = QoSDurabilityPolicy.VOLATILE
data_pub = node.create_publisher(Header, topic_name, qos_profile)
node_logger.info('Publishing on topic: {0}'.format(topic_name))
node_logger.info('Payload size: {0}'.format(args.payload_size))
data = 'a' * args.payload_size
msg = Header()
cycle_count = 0
def publish_msg(val):
msg.frame_id = '{0}_{1}'.format(val, data)
data_pub.publish(msg)
node_logger.info('Publishing: "{0}"'.format(val))
while rclpy.ok():
if args.end_after is not None and cycle_count >= args.end_after:
publish_msg(-1)
sleep(0.1) # allow reliable messages to get re-sent if needed
return
publish_msg(cycle_count)
cycle_count += 1
try:
sleep(args.period)
except KeyboardInterrupt:
publish_msg(-1)
sleep(0.1)
raise
def test_create_node(self):
node_name = 'create_node_test'
rclpy.create_node(node_name)
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_run.urdf"
urdfPath = get_prefix_path(
"mara_description") + "/share/mara_description/urdf/" + urdf
else:
urdf = "reinforcement_learning/mara_robot_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, urdfPath))
# 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]]) # offset
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'
BASE = 'base_robot'
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, 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 + 10
# 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.]
# Does not change anything
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 setUpClass(cls):
rclpy.init()
cls.node = rclpy.create_node('my_node', namespace='/my_ns')
def test_create_node_with_namespace(self):
node_name = 'create_node_test'
namespace = '/ns'
rclpy.create_node(node_name, namespace=namespace,
context=self.context).destroy_node()
def test_create_node_invalid_namespace(self):
node_name = 'create_node_test_invalid_namespace'
namespace = '/invalid_namespace?'
with self.assertRaisesRegex(InvalidNamespaceException,
'must not contain characters'):
rclpy.create_node(node_name, namespace=namespace)
def main(args=None):
if args is None:
args = sys.argv
rclpy.init(args)
node = rclpy.create_node('teleop_twist_keyboard')
pub = node.create_publisher(Twist, 'cmd_vel', qos_profile_default)
speed = 0.5
turn = 1.0
x = 0
y = 0
z = 0
th = 0
status = 0
try:
print(msg)
print(vels(speed, turn))
while (1):
key = getKey()
if key in moveBindings.keys():
x = moveBindings[key][0]
y = moveBindings[key][1]
z = moveBindings[key][2]
th = moveBindings[key][3]
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
print(vels(speed, turn))
if (status == 14):
print(msg)
status = (status + 1) % 15
else:
x = 0
y = 0
z = 0
th = 0
if (key == '\x03'):
break
twist = Twist()
twist.linear.x = x * speed
twist.linear.y = y * speed
twist.linear.z = z * speed
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = th * turn
pub.publish(twist)
except:
print(e)
finally:
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def test_create_node_invalid_name(self):
node_name = 'create_node_test_invalid_name?'
with self.assertRaisesRegex(InvalidNodeNameException,
'must not contain characters'):
rclpy.create_node(node_name, context=self.context)
