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)