def __init__(self, name, args=None):
super().__init__(name)
self.declare_parameter('synchronization', False)
self.declare_parameter('use_joint_state_publisher', False)
parser = argparse.ArgumentParser()
parser.add_argument(
'--webots-robot-name',
dest='webots_robot_name',
default='',
help='Specifies the "name" field of the robot in Webots.'
)
# Get robot name
arguments, _ = parser.parse_known_args()
if arguments.webots_robot_name:
os.environ['WEBOTS_ROBOT_NAME'] = arguments.webots_robot_name
self.robot = Supervisor()
self.timestep = int(self.robot.getBasicTimeStep())
self.__clock_publisher = self.create_publisher(Clock, 'clock', 10)
self.__step_service = self.create_service(SetInt, 'step', self.__step_callback)
self.__timer = self.create_timer((1 / MAX_REALTIME_FACTOR) * 1e-3 * self.timestep, self.__timer_callback)
self.__device_manager = None
# Joint state publisher
self.__joint_state_publisher = None
if self.get_parameter('use_joint_state_publisher').value:
self.__joint_state_publisher = JointStatePublisher(self.robot, '', self)
def __init__(self, args):
super().__init__('abb_driver', args=args)
prefix = self.get_parameter_or(
'prefix', Parameter('prefix', Parameter.Type.STRING, '')).value
self.jointStatePublisher = JointStatePublisher(self.robot, prefix,
self)
self.trajectoryFollower = TrajectoryFollower(self.robot,
self,
jointPrefix=prefix)
self.jointStateTimer = self.create_timer(0.001 * self.timestep,
self.joint_state_callback)
class ActionServerNode(WebotsNode):
def __init__(self, args):
super().__init__('abb_driver', args=args)
prefix = self.get_parameter_or(
'prefix', Parameter('prefix', Parameter.Type.STRING, '')).value
self.jointStatePublisher = JointStatePublisher(self.robot, prefix,
self)
self.trajectoryFollower = TrajectoryFollower(self.robot,
self,
jointPrefix=prefix)
self.jointStateTimer = self.create_timer(0.001 * self.timestep,
self.joint_state_callback)
def joint_state_callback(self):
# update joint state and trajectory follower
self.jointStatePublisher.publish()
def __init__(self,
name,
args=None,
enableTfPublisher=False,
enableJointState=False):
super().__init__(name)
self.declare_parameter('synchronization', False)
self.declare_parameter('use_joint_state_publisher', False)
parser = argparse.ArgumentParser()
parser.add_argument(
'--webots-robot-name',
dest='webotsRobotName',
default='',
help='Specifies the "name" field of the robot in Webots.')
# use 'parse_known_args' because ROS2 adds a lot of internal arguments
arguments, unknown = parser.parse_known_args()
if arguments.webotsRobotName:
os.environ['WEBOTS_ROBOT_NAME'] = arguments.webotsRobotName
self.robot = Supervisor()
self.timestep = int(self.robot.getBasicTimeStep())
self.clockPublisher = self.create_publisher(Clock, 'clock', 10)
timer_period = 0.001 * self.timestep # seconds
self.stepService = self.create_service(SetInt, 'step',
self.step_callback)
self.timer = self.create_timer(timer_period, self.timer_callback)
self.sec = 0
self.nanosec = 0
self.__device_manager = None
if enableTfPublisher:
if self.robot.getSupervisor():
self.tfPublisher = TfPublisher(self.robot, self)
else:
self.get_logger().warn(
'Impossible to publish transforms because the "supervisor"'
' field is false.')
if self.get_parameter(
'use_joint_state_publisher').value or enableJointState:
self.jointStatePublisher = JointStatePublisher(
self.robot, '', self)
def __init__(self, args):
super().__init__('example_controller', args)
self.get_logger().info("Init")
self.motor = self.robot.getMotor('motor1')
self.motor.setPosition(float('inf'))
self.motor.setVelocity(3.14 / 4)
self.sub = self.create_subscription(
Float32,
"cmd_vel",
self.set_vel,
qos_profile=qos_profile_sensor_data)
self.sub2 = self.create_subscription(
Vector3,
"vr/pos",
self.set_vel_from_pendant,
qos_profile=qos_profile_sensor_data)
self.motor.getPositionSensor().enable(100)
self.jsp = JointStatePublisher(self.robot, "", self)
self.timer = self.create_timer(0.01 * self.timestep, self.jsp.publish)
self.get_logger().info("Ready")
def start_joint_state_publisher(self):
"""Use `JointStatePublisher` to publish ROS2 messages of type `sensor_msgs/JointState`."""
self.__joint_state_publisher = JointStatePublisher(self.robot, '', self)
class WebotsNode(Node):
"""
Extends ROS2 base node to provide integration with Webots.
Args:
name (WebotsNode): Webots Robot node.
args (dict): Arguments passed to ROS2 base node.
"""
def __init__(self, name, args=None):
super().__init__(name)
self.declare_parameter('synchronization', False)
self.declare_parameter('use_joint_state_publisher', False)
parser = argparse.ArgumentParser()
parser.add_argument(
'--webots-robot-name',
dest='webots_robot_name',
default='',
help='Specifies the "name" field of the robot in Webots.'
)
# Get robot name
arguments, _ = parser.parse_known_args()
if arguments.webots_robot_name:
os.environ['WEBOTS_ROBOT_NAME'] = arguments.webots_robot_name
self.robot = Supervisor()
self.timestep = int(self.robot.getBasicTimeStep())
self.__clock_publisher = self.create_publisher(Clock, 'clock', 10)
self.__step_service = self.create_service(SetInt, 'step', self.__step_callback)
self.__timer = self.create_timer((1 / MAX_REALTIME_FACTOR) * 1e-3 * self.timestep, self.__timer_callback)
self.__device_manager = None
# Joint state publisher
self.__joint_state_publisher = None
if self.get_parameter('use_joint_state_publisher').value:
self.__joint_state_publisher = JointStatePublisher(self.robot, '', self)
def start_joint_state_publisher(self):
"""Use `JointStatePublisher` to publish ROS2 messages of type `sensor_msgs/JointState`."""
self.__joint_state_publisher = JointStatePublisher(self.robot, '', self)
def step(self, ms):
"""Call this method on each step."""
if self.__joint_state_publisher:
self.__joint_state_publisher.publish()
if self.__device_manager:
self.__device_manager.step()
if self.robot is None or self.get_parameter('synchronization').value:
return
# Robot step
if self.robot.step(ms) < 0.0:
del self.robot
self.robot = None
sys.exit(0)
# Update time
msg = Clock()
msg.clock = Time(seconds=self.robot.getTime()).to_msg()
self.__clock_publisher.publish(msg)
def __timer_callback(self):
self.step(self.timestep)
def __step_callback(self, request, response):
self.step(request.value)
response.success = True
return response
def start_device_manager(self, config=None):
"""
Start automatic ROSification of Webots devices available in the robot.
Kwargs:
config (dict): Dictionary of properties in format::
{
[device_name]: {
[property_name]: [property_value]
}
}
"""
self.__device_manager = DeviceManager(self, config)
class WebotsNode(Node):
def __init__(self,
name,
args=None,
enableTfPublisher=False,
enableJointState=False):
super().__init__(name)
self.declare_parameter('synchronization', False)
self.declare_parameter('use_joint_state_publisher', False)
parser = argparse.ArgumentParser()
parser.add_argument(
'--webots-robot-name',
dest='webotsRobotName',
default='',
help='Specifies the "name" field of the robot in Webots.')
# use 'parse_known_args' because ROS2 adds a lot of internal arguments
arguments, unknown = parser.parse_known_args()
if arguments.webotsRobotName:
os.environ['WEBOTS_ROBOT_NAME'] = arguments.webotsRobotName
self.robot = Supervisor()
self.timestep = int(self.robot.getBasicTimeStep())
self.clockPublisher = self.create_publisher(Clock, 'clock', 10)
timer_period = 0.001 * self.timestep # seconds
self.stepService = self.create_service(SetInt, 'step',
self.step_callback)
self.timer = self.create_timer(timer_period, self.timer_callback)
self.sec = 0
self.nanosec = 0
self.__device_manager = None
if enableTfPublisher:
if self.robot.getSupervisor():
self.tfPublisher = TfPublisher(self.robot, self)
else:
self.get_logger().warn(
'Impossible to publish transforms because the "supervisor"'
' field is false.')
if self.get_parameter(
'use_joint_state_publisher').value or enableJointState:
self.jointStatePublisher = JointStatePublisher(
self.robot, '', self)
def step(self, ms):
if self.get_parameter('use_joint_state_publisher').value:
self.jointStatePublisher.publish()
if self.__device_manager:
self.__device_manager.step()
if self.robot is None or self.get_parameter('synchronization').value:
return
# Robot step
if self.robot.step(ms) < 0.0:
del self.robot
self.robot = None
sys.exit(0)
# Update time
msg = Clock()
msg.clock = Time(seconds=self.robot.getTime()).to_msg()
self.clockPublisher.publish(msg)
def timer_callback(self):
self.step(self.timestep)
def step_callback(self, request, response):
self.step(request.value)
response.success = True
return response
def start_device_manager(self, config=None):
"""
Start automatic ROSification of Webots devices available in the robot.
Kwargs:
config (dict): Dictionary of properties in format::
{
[device_name]: {
[property_name]: [property_value]
}
}
"""
self.__device_manager = DeviceManager(self, config)