def __init__(self, node):
self.node = node
# Get robot name from parameter server, this is to ensure that the gazebo plugin subscribing to the control
# reads the same name as this code, because the topic depends on the robot name
robot_names = ut_param_server.get_robots(self.node)
self.robot_name = robot_names[0]
self.__joint_names = ut_param_server.get_joints(
self.node, self.robot_name)
joint_control_topic = '/' + self.robot_name + '/control'
self.__control_pub = self.node.create_publisher(
JointControl, joint_control_topic, qos_profile_sensor_data)
self.__joint_state_sub = self.node.create_subscription(
JointState,
"/joint_states",
self.__joint_state_subscription_callback,
qos_profile=qos_profile_parameters)
self.__contact_sub = self.node.create_subscription(
ContactsState,
f"/{self.robot_name}/contacts",
self.__contact_subscription_callback,
qos_profile=qos_profile_sensor_data)
self.__latest_contact_msg = None
self.__latest_joint_state_msg = None
self.__target_joint_state = numpy.array([0.0, 0.0, 0.0])
self.__previous_update_sim_time = rclpyTime()
self.__current_sim_time = rclpyTime()
self.__update_period_ns = 1000000000 / ut_param_server.get_update_rate(
self.node)
# Create reset client to call the reset service
self.__gazebo = GazeboConnection(self.node)
self.__time_lock = threading.Lock()
def __init__(self, node, robot_kwargs: Dict = None):
if robot_kwargs is None:
robot_kwargs = {}
self._gazebo = Gazebo()
super().__init__(node, robot_kwargs)
self._update_period_ns = 1000000000 / ut_param_server.get_update_rate(
self.node)
# Get robot name from parameter server, this is to ensure that the gazebo plugin subscribing to the control
# reads the same name as this code, because the topic depends on the robot name
robot_names = ut_param_server.get_robots(self.node)
self.robot_name = robot_names[0]
self._joint_names = ut_param_server.get_joints(self.node,
self.robot_name)
joint_control_topic = '/' + self.robot_name + '/control'
self._control_pub = self.node.create_publisher(JointControl,
joint_control_topic,
qos_profile_parameters)
self._contact_sub = self.node.create_subscription(
ContactsState,
f'/{self.robot_name}/contacts',
self.__contact_subscription_callback,
qos_profile=qos_profile_services_default)
self._latest_contact_msg = None
self._target_joint_state = numpy.array([0.0, 0.0, 0.0])
self._previous_update_sim_time = rclpyTime()
def __init__(self, node, state_noise_mu: float = None, state_noise_sigma: float = None):
self.node: rclpy.Node = node
self.state_noise_mu = state_noise_mu
self.state_noise_sigma = state_noise_sigma
self.__joint_state_sub = self.node.create_subscription(JointState, "/joint_states",
self.__joint_state_subscription_callback,
qos_profile=qos_profile_parameters)
self._latest_joint_state_msg = None
self._target_joint_state = numpy.array([0.0, 0.0, 0.0])
self._previous_update_sim_time = rclpyTime()
self._current_sim_time = rclpyTime()
self._update_period_ns = 1000000000 / ut_param_server.get_update_rate(self.node)