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 __reset_state(self) -> None: 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()
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)
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 _spin_until_update_period_over(self) -> None: # Loop to block such that when we take observation it is the latest observation when the # simulation is paused due to the gym training plugin # Also have a timeout such that when the loop gets stuck it will break itself out timeout_duration = 1.5 loop_start_time = time.time() while True: # spinning the node will cause self._current_sim_time to be updated rclpy.spin_once(self.node, timeout_sec=0.5) current_sim_time = copy.copy(self._current_sim_time) time_diff_ns = current_sim_time.nanoseconds - self._previous_update_sim_time.nanoseconds if time_diff_ns < 0: # print("Negative time difference detected, probably due to a reset") self._previous_update_sim_time = rclpyTime() time_diff_ns = current_sim_time.nanoseconds loop_end_time = time.time() loop_duration = loop_end_time - loop_start_time if time_diff_ns >= self._update_period_ns or loop_duration > timeout_duration: break if loop_duration >= timeout_duration: srv_time = self._get_current_sim_time_from_srv() self.node.get_logger().warn( f"Wait for simulation loop timeout, getting time from service instead" ) self.node.get_logger().warn( f'Current sim time: {current_sim_time.nanoseconds}, time from service: {srv_time.nanoseconds}' ) self._current_sim_time = srv_time for i in range(20): rclpy.spin_once(self.node, timeout_sec=0.1) current_sim_time = copy.copy(self._current_sim_time) self._previous_update_sim_time = current_sim_time
def __joint_state_subscription_callback(self, message: JointState) -> None: self._latest_joint_state_msg = message latest_msg_time = rclpyTime(seconds=message.header.stamp.sec, nanoseconds=message.header.stamp.nanosec) # print(f'[{message.header.stamp.sec}][{message.header.stamp.nanosec}]') if self._current_sim_time < latest_msg_time: self._current_sim_time = latest_msg_time
def __init__(self, node, state_noise_mu=None, state_noise_sigma=None, random_init_pos=False): self.node: rclpy.Node = node self.state_noise_mu = state_noise_mu self.state_noise_sigma = state_noise_sigma self.random_init_pos = random_init_pos print( f'-------------------------------Setting robot parameters-------------------------------' ) print( f'state_noise_mu: {self.state_noise_mu} # State noise mean (state noise follows gaussian distribution)' ) print( f'state_noise_sigma: {self.state_noise_sigma} # State noise standard deviation' ) print( f'random_init_pos: {self.random_init_pos} # Random initial positions' ) print( f'-------------------------------------------------------------------------------------' ) if self.state_noise_mu is not None: assert isinstance( self.state_noise_mu, float ), f'state_noise_mu should be a float, current type: {type(self.state_noise_mu)}' if self.state_noise_sigma is not None: assert isinstance( self.state_noise_sigma, float ), f'state_noise_sigma should be a float, current type: {type(self.state_noise_sigma)}' qos_profile = QoSProfile(reliability=1, depth=100) self.__joint_state_sub = self.node.create_subscription( JointState, '/joint_states', self.__joint_state_subscription_callback, qos_profile=qos_profile) # parameters = keep_last (1000), reliable, all else default 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._latest_contact_msg = None
def __init__(self, node, robot_kwargs): self.node: rclpy.Node = node self.state_noise_mu = robot_kwargs.get('state_noise_mu', 0.0) self.state_noise_sigma = robot_kwargs.get('state_noise_sigma', 0.0) self.random_init_pos = robot_kwargs.get('random_init_pos', False) qos_profile = QoSProfile(reliability=1, depth=100) self.__joint_state_sub = self.node.create_subscription( JointState, '/joint_states', self.__joint_state_subscription_callback, qos_profile=qos_profile) # parameters = keep_last (1000), reliable, all else default 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._latest_contact_msg = None
def _get_current_sim_time_from_srv(self) -> rclpyTime: client = self.node.create_client(GetCurrentSimTime, "/get_current_sim_time") req = GetCurrentSimTime.Request() retry_count = 0 while not client.wait_for_service(timeout_sec=1.0) and retry_count < 10: self.node.get_logger().info('/get_current_sim_time service not available, waiting again...') retry_count += 1 future = client.call_async(req) rclpy.spin_until_future_complete(self.node, future) if future.result() is not None: current_sim_time_sec = future.result().sec current_sim_time_nsec = future.result().nanosec current_sim_time = rclpyTime(seconds=current_sim_time_sec, nanoseconds=current_sim_time_nsec) return current_sim_time else: self.node.get_logger().warn('/get_current_sim_time service call failed') return rclpyTime()
def _reset_state(self) -> None: super()._reset_state() self._latest_contact_msg = None # We assume that if we do random initial position, the target joint state will be set after we call the random and the positions are obtained # So we only set target_joint_state when is not random_init_pos if not self.random_init_pos: print(f'Setting 0 initial positions') self._target_joint_state = numpy.array([0.0, 0.0, 0.0]) self._previous_update_sim_time = rclpyTime()
def __joint_state_subscription_callback(self, message: JointState) -> None: self.__latest_joint_state_msg = message with self.__time_lock: self.__current_sim_time = rclpyTime( seconds=message.header.stamp.sec, nanoseconds=message.header.stamp.nanosec)