def up_and_down(self, max_depth, min_depth, min_altitude):
body_velocity_req = BodyVelocityReq()
# header & goal
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_NORMAL
body_velocity_req.goal.requester = self.name + '_velocity'
body_velocity_req.twist.linear.z = 0.05
# Check if DoF is disable
body_velocity_req.disable_axis.x = True
body_velocity_req.disable_axis.y = True
body_velocity_req.disable_axis.z = False
body_velocity_req.disable_axis.roll = True
body_velocity_req.disable_axis.pitch = True
body_velocity_req.disable_axis.yaw = True
r = rospy.Rate(10)
while (self.depth < max_depth) and (self.altitude > min_altitude):
body_velocity_req.header.stamp = rospy.Time().now()
self.pub_bvr.publish(body_velocity_req)
r.sleep()
body_velocity_req.twist.linear.z = -0.05
while (self.depth > min_depth):
body_velocity_req.header.stamp = rospy.Time().now()
self.pub_bvr.publish(body_velocity_req)
r.sleep()
def requestBodyVelocity(vx, vy, vz, vyaw, pub):
# Function for steering the robot at the low level
#print ("(vx,vy,vz,vyaw) = ", vx,vy,vz,vyaw)
# Build BodyVelocityReq message according to params
message = BodyVelocityReq()
message.header.stamp = rospy.Time.now()
message.goal.priority = GoalDescriptor.PRIORITY_NORMAL_HIGH
message.goal.requester = rospy.get_name()
message.twist.linear.x = vx
message.twist.linear.y = vy
message.twist.linear.z = vz
message.twist.angular.z = vyaw
message.disable_axis.x = False
message.disable_axis.y = False
message.disable_axis.z = False
message.disable_axis.roll = True
message.disable_axis.pitch = True
message.disable_axis.yaw = False
# Publish built message
rospy.loginfo(rospy.get_name() + ": publishing BodyVelocityReq message")
pub.publish(message)
def enable_push_thread(self, f):
print 'thread: ', f
force = BodyForceReq()
force.header.frame_id = 'girona500'
force.goal.requester = self.name + '_force'
force.goal.priority = GoalDescriptor.PRIORITY_NORMAL
force.wrench.force.x = f
force.disable_axis.x = False
force.disable_axis.y = True
force.disable_axis.z = True
force.disable_axis.roll = True
force.disable_axis.pitch = True
force.disable_axis.yaw = True
vel = BodyVelocityReq()
vel.header.frame_id = 'girona500'
vel.goal.requester = self.name + '_vel'
vel.goal.priority = GoalDescriptor.PRIORITY_NORMAL
vel.disable_axis.x = True
vel.disable_axis.y = False
vel.disable_axis.z = False
vel.disable_axis.roll = False
vel.disable_axis.pitch = False
vel.disable_axis.yaw = False
rate = rospy.Rate(10)
while self.push_init:
print 'send ', f
force.header.stamp = rospy.Time().now()
vel.header.stamp = force.header.stamp
self.pub_body_force_req.publish(force)
self.pub_body_velocity_req.publish(vel)
rate.sleep()
def stepSH(self):
d = self.desired - self.state
u = 0.1 * numpy.dot(self.Kp, d)
yaw_ref = math.atan2(d[1], d[0])
yaw_error = (yaw_ref - self.stateHat.orientation.yaw) % (2 * math.pi)
if yaw_error >= math.pi:
yaw_error += -2 * math.pi
elif yaw_error <= -math.pi:
yaw_error += 2 * math.pi
pub = BodyVelocityReq()
pub.twist.linear.x = u[0] * math.cos(yaw_ref) + u[1] * math.sin(
yaw_ref)
pub.twist.angular.z = 0.3 * self.Kp[1][1] * yaw_error
'''Propagate integration after the output calculation'''
#self.internalState[0] += numpy.dot(self.Ki,d)*self.Ts + self.windup[0]*self.Kt(self.stateHat.body_velocity.x - pub.twist.linear.x)*self.Ts;
#self.internalState[1] += self.Ki[1][1]*yaw_error*self.Ts + self.windup[1]*self.Kt[1][1]*(self.stateHat.orientation_rate.yaw - pub.twist.angular.z)*self.Ts;
print "Windup:", u, pub.twist.linear.x, pub.twist.angular.z
pub.goal.priority = pub.goal.PRIORITY_NORMAL
pub.disable_axis.y = pub.disable_axis.pitch = 0
pub.disable_axis.roll = pub.disable_axis.z = 0
self.out.publish(pub)
def stepSS(self):
d = self.desired - self.state
u = numpy.dot(numpy.transpose(self.R),
(numpy.dot(self.Kp, d) + self.internalState + self.ff))
ti = numpy.dot(numpy.linalg.pinv(self.Bstar), u)
print "Desired speed:", u
print "Desired forces:", ti
scale = numpy.max(
numpy.array([
numpy.abs(ti[0]) / self.tmax,
numpy.abs(ti[1]) / self.tmax, 1
]))
ti = ti / scale
print "Scaled forces:", ti
ddu = numpy.dot(self.Bstar, ti)
print "Scaled speed:", ddu
'''Ignore scaling'''
ddu = u
#ddu = numpy.dot(numpy.transpose(self.R),numpy.array([1.0,1.0], dtype=numpy.float32));
#ddu[0] = u[0] / scale;
#ddu[1] = u[1] / scale;
du = numpy.array(
[self.stateHat.body_velocity.x, self.stateHat.body_velocity.y],
dtype=numpy.float32)
#ddu = u;
'''Propagate integration after the output calculation'''
#if numpy.linalg.norm(du,2) == 0 :
if numpy.linalg.norm(self.windup, 2) == 0:
#self.internalState += self.windup*numpy.dot(self.R,numpy.dot(self.Kt,du-u)*self.Ts);
#self.internalState += numpy.dot(self.R,numpy.dot(self.Kt,du-u)*self.Ts);
#if self.windup[0]: u[0] = du[0];
#if self.windup[1]: u[1] = du[1];
self.internalState += numpy.dot(
self.Ki, d) * self.Ts + 0 * numpy.dot(
self.R,
numpy.dot(self.Kt, ddu - u) * self.Ts)
'''Ignore scaling'''
#self.internalState += numpy.dot(self.Ki,d)*self.Ts + numpy.dot(self.R,numpy.dot(self.Kt,ddu-u)*self.Ts);
# print "No windup"
#else:
self.uk_1 = u
print "Windup:", self.windup, u
#u=ddu;
pub = BodyVelocityReq()
pub.twist.linear.x = u[0]
pub.twist.linear.y = u[1]
pub.goal.priority = pub.goal.PRIORITY_NORMAL
pub.disable_axis.yaw = pub.disable_axis.pitch = 0
pub.disable_axis.roll = pub.disable_axis.z = 0
self.out.publish(pub)
def step(self):
out = BodyVelocityReq()
out.twist.linear.x = self._speedGenX.step(
(rospy.Time.now() - self._lastTime).to_sec())
out.twist.linear.y = self._speedGenY.step(
(rospy.Time.now() - self._lastTime).to_sec())
self._lastTime = rospy.Time.now()
self._bspdreq.publish(out)
'''
def set_zero_velocity(self, event):
""" Send zero velocity requests if the vehicle is below the
desired depth """
self.lock.acquire()
if self.navigation.position.depth > self.set_zero_velocity_depth:
bvr = BodyVelocityReq()
bvr.twist.linear.x = 0.0
bvr.twist.linear.y = 0.0
bvr.twist.linear.z = 0.0
bvr.twist.angular.x = 0.0
bvr.twist.angular.y = 0.0
bvr.twist.angular.z = 0.0
bvr.goal.priority = GoalDescriptor.PRIORITY_SAFETY_LOW
bvr.header.stamp = rospy.Time.now()
for i in range(len(self.set_zero_velocity_axis)):
if self.current_enabled_axis[0]:
bvr.disable_axis.x = True
else:
bvr.disable_axis.x = self.set_zero_velocity_axis[i][0]
if self.current_enabled_axis[1]:
bvr.disable_axis.y = True
else:
bvr.disable_axis.y = self.set_zero_velocity_axis[i][1]
if self.current_enabled_axis[2]:
bvr.disable_axis.z = True
else:
bvr.disable_axis.z = self.set_zero_velocity_axis[i][2]
if self.current_enabled_axis[3]:
bvr.disable_axis.roll = True
else:
bvr.disable_axis.roll = self.set_zero_velocity_axis[i][3]
if self.current_enabled_axis[4]:
bvr.disable_axis.pitch = True
else:
bvr.disable_axis.pitch = self.set_zero_velocity_axis[i][4]
if self.current_enabled_axis[5]:
bvr.disable_axis.yaw = True
else:
bvr.disable_axis.yaw = self.set_zero_velocity_axis[i][5]
# Set Zero Velocity
bvr.goal.requester = 'set_zero_velocity_' + str(i)
self.pub_body_velocity_req.publish(bvr)
# Disable all axis
self.current_enabled_axis = [False, False, False, False, False, False]
self.lock.release()
def check_map_ack(self, event):
""" This is a callback for a timer. It publishes ack safety message
and pose and velocity safety messages if map_ack is lost """
if self.map_ack_init:
self.diagnostic.add(
"last_ack", str(rospy.Time.now().to_sec() - self.last_map_ack))
if self.map_ack_alive:
self.map_ack_alive = False
self.diagnostic.setLevel(DiagnosticStatus.OK)
else:
rospy.loginfo("%s: we have lost map_ack!", self.name)
self.diagnostic.setLevel(DiagnosticStatus.WARN,
'Communication with map_ack lost!')
body_velocity_req = BodyVelocityReq()
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION_LOW
body_velocity_req.goal.requester = self.name + '_vel'
body_velocity_req.twist.linear.x = 0.0
body_velocity_req.twist.linear.y = 0.0
body_velocity_req.twist.linear.z = 0.0
body_velocity_req.twist.angular.x = 0.0
body_velocity_req.twist.angular.y = 0.0
body_velocity_req.twist.angular.z = 0.0
body_velocity_req.disable_axis.x = True
body_velocity_req.disable_axis.y = True
body_velocity_req.disable_axis.z = True
body_velocity_req.disable_axis.roll = True
body_velocity_req.disable_axis.pitch = True
body_velocity_req.disable_axis.yaw = True
body_velocity_req.header.stamp = rospy.Time().now()
self.pub_body_velocity_req.publish(body_velocity_req)
world_waypoint_req = WorldWaypointReq()
world_waypoint_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION_LOW
world_waypoint_req.goal.requester = self.name + '_pose'
world_waypoint_req.disable_axis.x = True
world_waypoint_req.disable_axis.y = True
world_waypoint_req.disable_axis.z = True
world_waypoint_req.disable_axis.roll = True
world_waypoint_req.disable_axis.pitch = True
world_waypoint_req.disable_axis.yaw = True
world_waypoint_req.header.stamp = rospy.Time().now()
self.pub_world_waypoint_req.publish(world_waypoint_req)
else:
rospy.loginfo("%s: waiting for map ack...", self.name)
# Send ack message
msg = String()
msg.data = str(self.seq) + ' ok'
self.pub_check_joystick.publish(msg)
def update_nav_sts(self, nav):
"""Navigation callback. It triggers the main loop."""
self.diagnostic.add("altitude", str(nav.altitude))
self.diagnostic.add("depth", str(nav.position.depth))
if (nav.altitude > 0 and nav.altitude < self.min_altitude
and nav.position.depth > 0.5) or (nav.position.depth >
self.max_depth):
# Show message
self.diagnostic.setLevel(
DiagnosticStatus.WARN,
'Invalid depth/altitude! Moving vehicle up.')
if (nav.altitude > 0 and nav.altitude < self.min_altitude
and nav.position.depth > 0.5):
rospy.logwarn("%s: invalid altitude: %s", self.name,
nav.altitude)
if (nav.position.depth > self.max_depth):
rospy.logwarn("%s: invalid depth: %s", self.name,
nav.position.depth)
# Go up
bvr = BodyVelocityReq()
bvr.twist.linear.x = 0.0
bvr.twist.linear.y = 0.0
bvr.twist.linear.z = -0.5
bvr.twist.angular.x = 0.0
bvr.twist.angular.y = 0.0
bvr.twist.angular.z = 0.0
bvr.disable_axis.x = True
bvr.disable_axis.y = True
bvr.disable_axis.z = False
bvr.disable_axis.roll = True
bvr.disable_axis.pitch = True
# If yaw is True the vehicle can rotate while it goes up
# but if it is False then it can not be teleoperated
bvr.disable_axis.yaw = True
bvr.goal.priority = GoalDescriptor.PRIORITY_SAFETY_HIGH
bvr.goal.requester = self.name
bvr.header.stamp = rospy.Time.now()
self.pub_body_velocity_req.publish(bvr)
else:
self.diagnostic.setLevel(DiagnosticStatus.OK)
def __compute_yaw_rate__(self, y_offset):
# Publish Body Velocity Request
body_velocity_req = BodyVelocityReq()
# header & goal
body_velocity_req.header.stamp = rospy.Time().now()
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_NORMAL
body_velocity_req.goal.requester = self.name + '_velocity'
# twist set-point
body_velocity_req.twist.angular.z = y_offset / 4.0
# Check if DoF is disable
body_velocity_req.disable_axis.x = True
body_velocity_req.disable_axis.y = True
body_velocity_req.disable_axis.z = True
body_velocity_req.disable_axis.roll = True
body_velocity_req.disable_axis.pitch = True
body_velocity_req.disable_axis.yaw = False
return body_velocity_req
def __compute_yaw_rate__(self, y_offset, x_offset):
# Publish Body Velocity Request
body_velocity_req = BodyVelocityReq()
distance = math.sqrt(y_offset**2 + x_offset**2)
#print 'distance compute yaw: ', distance
if distance > 1.5:
# twist set-point
body_velocity_req.twist.angular.z = y_offset / 4.0
#print '>>>>>>>>>>>>>>>>>>>>>>>bigger than 1.5 m'
else:
body_velocity_req.twist.angular.z = cola2_lib.normalizeAngle(
self.chain_orientation - self.current_yaw) / 10.0
#print '>>>>>>>>>>>>>>>>>>>>>>>smaller than 1.5 m'
#print 'Chain orientation:', self.chain_orientation, ' Current yaw: ', self.current_yaw
if body_velocity_req.twist.angular.z > 0.15:
body_velocity_req.twist.angular.z = 0.15
elif body_velocity_req.twist.angular.z < -0.15:
body_velocity_req.twist.angular.z = -0.15
# header & goal
body_velocity_req.header.stamp = rospy.Time().now()
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_NORMAL
body_velocity_req.goal.requester = self.name + '_velocity'
# Check if DoF is disable
body_velocity_req.disable_axis.x = True
body_velocity_req.disable_axis.y = True
body_velocity_req.disable_axis.z = True
body_velocity_req.disable_axis.roll = True
body_velocity_req.disable_axis.pitch = True
body_velocity_req.disable_axis.yaw = False
return body_velocity_req
def __init__(self, name):
self.name = name
# Default parameters
self.window_length = 3.0
self.window_start = 1.0
self.waypoint_req = WorldWaypointReq()
self.body_velocity_req = BodyVelocityReq()
self.odometry = Odometry()
self.sonar_img_pose = PoseStamped()
self.lock = threading.RLock()
self.last_waypoint = WorldWaypointReq()
self.last_waypoint_update = rospy.Time.now().to_sec()
self.look_around = False
self.yaw_offset = 0.45 # 0.35 => 25 degrees
self.do_turn_around = False
self.lock = threading.RLock()
self.listener = tf.TransformListener()
self.broadcaster = tf.TransformBroadcaster()
self.odometry_updated = False
self.big_turn_around = False
# self.get_config()
self.pub_yaw_rate = rospy.Publisher('/cola2_control/body_velocity_req',
BodyVelocityReq)
self.pub_waypoint_req = rospy.Publisher(
'/cola2_control/world_waypoint_req', WorldWaypointReq)
self.pub_marker = rospy.Publisher('/udg_pandora/orientation_link',
Marker)
# Create Subscriber Updates (z)
rospy.Subscriber('/udg_pandora/link_waypoints',
MarkerArray,
self.sonar_waypoint_update,
queue_size=1)
rospy.Subscriber('/cola2_perception/soundmetrics_aris3000/sonar_info',
SonarInfo,
self.sonar_info_update,
queue_size=1)
rospy.Subscriber('/udg_pandora/world_waypoint_req',
WorldWaypointReq,
self.world_waypoint_req_update,
queue_size=1)
rospy.Subscriber('/pose_ekf_slam/odometry',
Odometry,
self.odometry_update,
queue_size=1)
rospy.Subscriber(
'/cola2_perception/soundmetrics_aris3000/sonar_img_pose',
PoseStamped,
self.sonar_img_pose_update,
queue_size=1)
rospy.Timer(rospy.Duration(0.05), self.publish_control)
rospy.Timer(rospy.Duration(0.5), self.update_sonar_img_tf)
def stepSSbackward(self):
d = self.desired - self.state
#ti = numpy.dot(numpy.linalg.pinv(numpy.dot(numpy.linalg.inv(self.Kp),self.Bstar)),d);
#print "Desired error:",d
#print "Desired forces:",ti
#scale = numpy.max(numpy.array([numpy.abs(ti[0])/self.tmax,numpy.abs(ti[1])/self.tmax,1]));
#ti = ti/scale;
#print "Scaled forces:",ti
#dd = numpy.dot(numpy.dot(numpy.linalg.inv(self.Kp),self.Bstar),ti);
#print "Scaled error:",dd
#d=dd;
self.internalState += numpy.dot(self.Kp, d) - self.lastP
self.lastP = numpy.dot(self.Kp, d)
self.internalState += numpy.dot(self.R, self.ff) - self.lastFF
self.lastFF = numpy.dot(self.R, self.ff)
print "Feed forward:", self.lastFF
if (numpy.linalg.norm(self.lastW, 2)
== 0) and (numpy.linalg.norm(self.windup, 2) != 0):
print "Oduzimanje."
self.internalState -= self.lastI
self.lastI = 0
if numpy.linalg.norm(self.windup, 2) == 0:
print "Dodavanje"
self.internalState += numpy.dot(self.Ki, d) * self.Ts
self.lastI += numpy.dot(self.Ki, d) * self.Ts
self.lastW = 1 * self.windup
u = numpy.dot(numpy.transpose(self.R), self.internalState)
du = numpy.array(
[self.stateHat.body_velocity.x, self.stateHat.body_velocity.y],
dtype=numpy.float32)
#du = numpy.dot(self.R,du);
#ti = numpy.dot(numpy.linalg.pinv(self.Bstar),u);
#print "Desired speed:",u
#print "Desired forces:",ti
#scale = numpy.max(numpy.array([numpy.abs(ti[0])/self.tmax,numpy.abs(ti[1])/self.tmax,1]));
#ti = ti/scale;
#print "Scaled forces:",ti
#ddu = numpy.dot(self.Bstar,ti);
#print "Scaled speed:",ddu
#if (ddu[0] != u[0]) or (ddu[1] != u[1]):
#if numpy.linalg.norm(self.windup, 2):
# self.internalState = numpy.dot(numpy.linalg.inv(numpy.identity(2, dtype=numpy.float32)+self.Kt*self.Ts),(self.internalState +
# numpy.dot(self.R,numpy.dot(self.Kt,ddu)*self.Ts) + numpy.dot(self.R,numpy.dot(self.Kt,du)*self.Ts)));
#self.internalState = numpy.dot(numpy.linalg.inv(numpy.identity(2, dtype=numpy.float32)+self.Kt*self.Ts),(self.internalState +
# numpy.dot(self.R,numpy.dot(self.Kt,ddu)*self.Ts)));
print "Windup:", self.windup, u
'''The velocity only stuff'''
#if numpy.linalg.norm(self.windup, 2):
# self.internalState = numpy.dot(numpy.linalg.inv(numpy.identity(2, dtype=numpy.float32)+self.Kt*self.Ts),(self.internalState +
# numpy.dot(self.Kt,du)*self.Ts));
u = numpy.dot(numpy.transpose(self.R), self.internalState)
#u = numpy.dot(numpy.transpose(self.R),ddu);
pub = BodyVelocityReq()
pub.twist.linear.x = u[0]
pub.twist.linear.y = u[1]
pub.goal.priority = pub.goal.PRIORITY_NORMAL
pub.disable_axis.yaw = pub.disable_axis.pitch = 0
pub.disable_axis.roll = pub.disable_axis.z = 0
self.out.publish(pub)
def map_ack_data_callback(self, data):
""" This is the main callback. Data is recieved, processed and sent
to pose and velocity controllers """
# Compute desired positions and velocities
desired = [0 for x in range(12)]
for i in range(6):
if (data.axes[i] < 0):
desired[i] = abs(
data.axes[i]) * self.min_pos[i] + self.base_pose[i]
else:
desired[i] = data.axes[i] * self.max_pos[i] + self.base_pose[i]
if i > 2:
# Normalize angles
desired[i] = cola2_lib.normalizeAngle(desired[i])
for i in range(6, 12):
if (data.axes[i] < 0):
desired[i] = abs(data.axes[i]) * self.min_vel[i - 6]
else:
desired[i] = data.axes[i] * self.max_vel[i - 6]
# Check if pose controller is enabled
for b in range(6):
if data.buttons[b] == 1:
self.pose_controlled_axis[b] = True
if self.actualize_base_pose:
self.base_pose[b] = self.last_pose[b]
rospy.loginfo("%s: axis %s now is pose", self.name, str(b))
# Check if velocity controller is enabled
for b in range(6, 12):
if data.buttons[b] == 1:
self.pose_controlled_axis[b - 6] = False
rospy.loginfo("%s: axis %s now is velocity", self.name,
str(b - 6))
if self.nav_init:
# Positions
world_waypoint_req = WorldWaypointReq()
world_waypoint_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION
world_waypoint_req.goal.requester = self.name + '_pose'
world_waypoint_req.position.north = desired[0]
world_waypoint_req.position.east = desired[1]
world_waypoint_req.position.depth = desired[2]
world_waypoint_req.orientation.roll = desired[3]
world_waypoint_req.orientation.pitch = desired[4]
world_waypoint_req.orientation.yaw = desired[5]
world_waypoint_req.disable_axis.x = not self.pose_controlled_axis[0]
world_waypoint_req.disable_axis.y = not self.pose_controlled_axis[1]
world_waypoint_req.disable_axis.z = not self.pose_controlled_axis[2]
world_waypoint_req.disable_axis.roll = not self.pose_controlled_axis[
3]
world_waypoint_req.disable_axis.pitch = not self.pose_controlled_axis[
4]
world_waypoint_req.disable_axis.yaw = not self.pose_controlled_axis[
5]
world_waypoint_req.header.stamp = rospy.Time().now()
# if not world_waypoint_req.disable_axis.pitch:
# rospy.logfatal("%s: PITCH IS NOT DISABLED!", self.name)
# world_waypoint_req.disable_axis.pitch = True
if (world_waypoint_req.disable_axis.x
and world_waypoint_req.disable_axis.y
and world_waypoint_req.disable_axis.z
and world_waypoint_req.disable_axis.roll
and world_waypoint_req.disable_axis.pitch
and world_waypoint_req.disable_axis.yaw):
world_waypoint_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION_LOW
self.pub_world_waypoint_req.publish(world_waypoint_req)
# Velocities
body_velocity_req = BodyVelocityReq()
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION
body_velocity_req.goal.requester = self.name + '_vel'
body_velocity_req.twist.linear.x = desired[6]
body_velocity_req.twist.linear.y = desired[7]
body_velocity_req.twist.linear.z = desired[8]
body_velocity_req.twist.angular.x = desired[9]
body_velocity_req.twist.angular.y = desired[10]
body_velocity_req.twist.angular.z = desired[11]
body_velocity_req.disable_axis.x = self.pose_controlled_axis[0]
body_velocity_req.disable_axis.y = self.pose_controlled_axis[1]
body_velocity_req.disable_axis.z = self.pose_controlled_axis[2]
body_velocity_req.disable_axis.roll = self.pose_controlled_axis[3]
body_velocity_req.disable_axis.pitch = self.pose_controlled_axis[4]
body_velocity_req.disable_axis.yaw = self.pose_controlled_axis[5]
# Check if DoF is disable
if abs(body_velocity_req.twist.linear.x) < 0.025:
body_velocity_req.disable_axis.x = True
if abs(body_velocity_req.twist.linear.y) < 0.025:
body_velocity_req.disable_axis.y = True
if abs(body_velocity_req.twist.linear.z) < 0.025:
body_velocity_req.disable_axis.z = True
if abs(body_velocity_req.twist.angular.x) < 0.01:
body_velocity_req.disable_axis.roll = True
if abs(body_velocity_req.twist.angular.y) < 0.01:
body_velocity_req.disable_axis.pitch = True
if abs(body_velocity_req.twist.angular.z) < 0.01:
body_velocity_req.disable_axis.yaw = True
# If all DoF are disabled set priority to LOW
if (body_velocity_req.disable_axis.x
and body_velocity_req.disable_axis.y
and body_velocity_req.disable_axis.z
and body_velocity_req.disable_axis.roll
and body_velocity_req.disable_axis.pitch
and body_velocity_req.disable_axis.yaw):
body_velocity_req.goal.priority = GoalDescriptor.PRIORITY_TELEOPERATION_LOW
# Publish message
body_velocity_req.header.stamp = rospy.Time().now()
self.pub_body_velocity_req.publish(body_velocity_req)