def _move(self, left_x, left_y, right_x): """Commands the robot with a velocity command based on left/right stick values. Args: left_x: X value of left stick. left_y: Y value of left stick. right_x: X value of right stick. """ # Stick left_x controls robot v_y v_y = -left_x * VELOCITY_BASE_SPEED # Stick left_y controls robot v_x v_x = left_y * VELOCITY_BASE_SPEED # Stick right_x controls robot v_rot v_rot = -right_x * VELOCITY_BASE_ANGULAR # Recreate mobility_params with the latest information self.mobility_params = RobotCommandBuilder.mobility_params( body_height=self.body_height, locomotion_hint=self.mobility_params.locomotion_hint, stair_hint=self.mobility_params.stair_hint) cmd = RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot, params=self.mobility_params) self.command_client.robot_command_async(cmd, end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def velocity_cmd_listener(self, twist): """Callback that sends instantaneous velocity [m/s] commands to Spot""" MAX_MARCHING_TIME = 5 v_x = twist.linear.x v_y = twist.linear.y v_rot = twist.angular.z all_zero = (v_x == 0) and (v_y == 0) and (v_rot == 0) if (all_zero): if self.t_last_non_zero_cmd < time.monotonic() - MAX_MARCHING_TIME: # No need to send command, just return. return else: self.t_last_non_zero_cmd = time.monotonic() cmd = RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot) try: self.command_client.robot_command(cmd, end_time_secs=time.time() + self.VELOCITY_CMD_DURATION) except: print("Invalid command: v_x=${},v_y=${},v_rot${}".format( v_x, v_y, v_rot)) # rospy.loginfo( # "Robot velocity cmd sent: v_x=${},v_y=${},v_rot${}".format(v_x, v_y, v_rot)) return []
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0): self._start_robot_command(desc, RobotCommandBuilder.velocity_command( v_x=v_x, v_y=v_y, v_rot=v_rot, body_height=self.height, locomotion_hint=spot_command_pb2.HINT_AMBLE), end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def test_velocity_command(): v_x = 1.0 v_y = 2.0 v_rot = 3.0 command = RobotCommandBuilder.velocity_command(v_x, v_y, v_rot) _test_has_mobility(command) assert command.mobility_command.HasField("se2_velocity_request") vel_cmd = command.mobility_command.se2_velocity_request assert vel_cmd.velocity.linear.x == v_x assert vel_cmd.velocity.linear.y == v_y assert vel_cmd.velocity.angular == v_rot assert vel_cmd.frame.base_frame == geometry_pb2.FRAME_BODY
def test_velocity_command(): v_x = 1.0 v_y = 2.0 v_rot = 3.0 command = RobotCommandBuilder.velocity_command(v_x, v_y, v_rot) _test_has_mobility_deprecated(command) assert command.mobility_command.HasField("se2_velocity_request") vel_cmd = command.mobility_command.se2_velocity_request assert vel_cmd.velocity.linear.x == v_x assert vel_cmd.velocity.linear.y == v_y assert vel_cmd.velocity.angular == v_rot assert vel_cmd.se2_frame_name == BODY_FRAME_NAME
def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.1): """Send a velocity motion command to the robot. Args: v_x: Velocity in the X direction in meters v_y: Velocity in the Y direction in meters v_rot: Angular velocity around the Z axis in radians cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate). """ end_time = time.time() + cmd_duration self._robot_command(RobotCommandBuilder.velocity_command( v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params), end_time_secs=end_time) self._last_motion_command_time = end_time
def velocity_cmd_srv(self, twist): """Callback that sends instantaneous velocity [m/s] commands to Spot""" v_x = twist.velocity.linear.x v_y = twist.velocity.linear.y v_rot = twist.velocity.angular.z cmd = RobotCommandBuilder.velocity_command( v_x=v_x, v_y=v_y, v_rot=v_rot ) self.command_client.robot_command( cmd, end_time_secs=time.time() + self.VELOCITY_CMD_DURATION ) rospy.loginfo( "Robot velocity cmd sent: v_x=${},v_y=${},v_rot${}".format(v_x, v_y, v_rot)) return []
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0): self._start_robot_command( desc, RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y, v_rot=v_rot), end_time_secs=time.time() + VELOCITY_CMD_DURATION)