def request_wrench_cb(self, msg):
'''Callback for requesting a wrench'''
time_now = rospy.Time.now()
if (time_now - self.last_command_time) < self._min_command_time:
return
else:
self.last_command_time = rospy.Time.now()
force = rosmsg_to_numpy(msg.wrench.force)
torque = rosmsg_to_numpy(msg.wrench.torque)
wrench = np.hstack([force, torque])
success = False
while not success:
u, success = self.map(wrench)
if not success:
# If we fail to compute, shrink the wrench
wrench = wrench * 0.75
continue
thrust_cmds = []
# Assemble the list of thrust commands to send
for name, thrust in zip(self.thruster_name_map, u):
# > Can speed this up by avoiding appends
if name in self.dropped_thrusters:
continue # Ignore dropped thrusters
if np.fabs(thrust) < self.min_commandable_thrust:
thrust = 0
thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
# If the sub is not killed
if self.killed is False:
self.thruster_pub.publish(thrust_cmds)
def request_wrench_cb(self, msg):
'''Callback for requesting a wrench'''
time_now = rospy.Time.now()
if (time_now - self.last_command_time) < self._min_command_time:
return
else:
self.last_command_time = rospy.Time.now()
force = rosmsg_to_numpy(msg.wrench.force)
torque = rosmsg_to_numpy(msg.wrench.torque)
wrench = np.hstack([force, torque])
success = False
while not success:
u, success = self.map(wrench)
if not success:
# If we fail to compute, shrink the wrench
wrench = wrench * 0.75
continue
thrust_cmds = []
# Assemble the list of thrust commands to send
for name, thrust in zip(self.thruster_name_map, u):
# > Can speed this up by avoiding appends
if name in self.dropped_thrusters:
continue # Ignore dropped thrusters
if np.fabs(thrust) < self.min_commandable_thrust:
thrust = 0
thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
# If the sub is not killed
if self.killed is False:
self.thruster_pub.publish(thrust_cmds)
def twist_cb(self, msg):
if self.cur_orientation is None or self.cur_position is None:
return
linear = sub8_utils.rosmsg_to_numpy(msg.linear)
angular = sub8_utils.rosmsg_to_numpy(msg.angular)
self.target_position += self.target_orientation.dot(self._position_gain * linear)
self.diff_position = np.subtract(self.cur_position, self.target_position)
if self.mode == '2d':
# Ignore 6-dof shenanigans
angular[0] = 0
angular[1] = 0
gained_angular = self._orientation_gain * angular
skewed = sub8_utils.skew_symmetric_cross(gained_angular)
rotation = linalg.expm(skewed)
# TODO: Better
# self.target_orientation = self.cur_orientation
# self.target_orientation = rotation.dot(self.target_orientation)
self.target_orientation = self.target_orientation.dot(rotation)
self.target_distance = round(np.linalg.norm(np.array([self.diff_position[0], self.diff_position[1]])), 3)
self.target_depth = round(self.diff_position[2], 3)
self.target_orientation = self.target_orientation.dot(rotation)
blank = np.eye(4)
blank[:3, :3] = self.target_orientation
self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(blank)
self.publish_target_pose(self.target_position, self.target_orientation_quaternion)
def test_make_wrench_stamped(self):
'''Test that wrenchmaking works'''
for k in range(10):
force = np.random.random(3) * 10
torque = np.random.random(3) * 10
wrench_msg = make_wrench_stamped(force, torque, frame='/enu')
msg_force = rosmsg_to_numpy(wrench_msg.wrench.force)
msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque)
def test_make_wrench_stamped(self):
'''Test that wrenchmaking works'''
for k in range(10):
force = np.random.random(3) * 10
torque = np.random.random(3) * 10
wrench_msg = make_wrench_stamped(force, torque, frame='/enu')
msg_force = rosmsg_to_numpy(wrench_msg.wrench.force)
msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque)
def twist_cb(self, msg):
linear = sub8_utils.rosmsg_to_numpy(msg.linear)
angular = sub8_utils.rosmsg_to_numpy(msg.angular)
if self.behavior == 'wrench':
# Directly transcribe linear and angular 'velocities' to torque
# TODO: Setup actual TF!
self.wrench_pub.publish(
sub8_utils.make_wrench_stamped(
force=self.linear_gain * self.world_to_body.dot(linear),
torque=self.angular_gain * self.world_to_body.dot(angular)
)
)
else:
raise(NotImplementedError("Velocity and position control by mouse are not yet supported"))
def test_rosmsg_to_numpy_quaternion(self):
'''Test a rosmsg conversion for a geometry_msgs/Quaternion'''
# I know this is not a unit quaternion
q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2)
numpy_array = rosmsg_to_numpy(q)
np.testing.assert_allclose(np.array([0.7, 0.7, 0.1, 0.2]), numpy_array)
def request_wrench_cb(self, msg):
'''Callback for requesting a wrench'''
force = rosmsg_to_numpy(msg.wrench.force)
torque = rosmsg_to_numpy(msg.wrench.torque)
wrench = np.hstack([force, torque])
u, success = self.map(wrench)
if success:
thrust_cmds = []
# Assemble the list of thrust commands to send
for name, thrust in zip(self.thruster_name_map, u):
# > Can speed this up by avoiding appends
thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
self.thruster_pub.publish(thrust_cmds)
else:
# I expect this to happen frequently enough that we should not raise
rospy.logwarn("Could not achieve wrench of {}".format(wrench))
def request_wrench_cb(self, msg):
'''Callback for requesting a wrench'''
force = rosmsg_to_numpy(msg.wrench.force)
torque = rosmsg_to_numpy(msg.wrench.torque)
wrench = np.hstack([force, torque])
u, success = self.map(wrench)
if success:
thrust_cmds = []
# Assemble the list of thrust commands to send
for name, thrust in zip(self.thruster_name_map, u):
# > Can speed this up by avoiding appends
thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
self.thruster_pub.publish(thrust_cmds)
else:
# I expect this to happen frequently enough that we should not raise
rospy.logwarn("Could not achieve wrench of {}".format(wrench))
def twist_cb(self, msg):
linear = sub8_utils.rosmsg_to_numpy(msg.linear)
angular = sub8_utils.rosmsg_to_numpy(msg.angular)
if self.behavior == 'wrench':
# Directly transcribe linear and angular 'velocities' to torque
# TODO: Setup actual TF!
self.wrench_pub.publish(
sub8_utils.make_wrench_stamped(
force=self.linear_gain * self.world_to_body.dot(linear),
torque=self.angular_gain *
self.world_to_body.dot(angular)))
else:
raise (NotImplementedError(
"Velocity and position control by mouse are not yet supported")
)
def test_rosmsg_to_numpy_custom(self):
'''Test a rosmsg conversion for a custom msg'''
pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14)
numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta'])
np.testing.assert_allclose(
np.array([1.0, 2.0, 3.14]),
numpy_array
)
def test_rosmsg_to_numpy_vector(self):
'''Test a rosmsg conversion for a geometry_msgs/Vector'''
v = Vector3(x=0.1, y=99., z=21.)
numpy_array = rosmsg_to_numpy(v)
np.testing.assert_allclose(
np.array([0.1, 99., 21.]),
numpy_array
)
def test_rosmsg_to_numpy_quaternion(self):
'''Test a rosmsg conversion for a geometry_msgs/Quaternion'''
# I know this is not a unit quaternion
q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2)
numpy_array = rosmsg_to_numpy(q)
np.testing.assert_allclose(
np.array([0.7, 0.7, 0.1, 0.2]),
numpy_array
)
def twist_cb(self, msg):
if self.cur_orientation is None or self.cur_position is None:
return
linear = sub8_utils.rosmsg_to_numpy(msg.linear)
angular = sub8_utils.rosmsg_to_numpy(msg.angular)
self.target_position += self.target_orientation.dot(self._position_gain * linear)
gained_angular = self._orientation_gain * angular
skewed = sub8_utils.skew_symmetric_cross(gained_angular)
rotation = linalg.expm(skewed)
# TODO: Better
# self.target_orientation = self.cur_orientation
# self.target_orientation = rotation.dot(self.target_orientation)
self.target_orientation = self.target_orientation.dot(rotation)
blank = np.eye(4)
blank[:3, :3] = self.target_orientation
self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(blank)
self.publish_target_pose(self.target_position, self.target_orientation_quaternion)
def twist_cb(self, msg):
if self.cur_orientation is None or self.cur_position is None:
return
linear = sub8_utils.rosmsg_to_numpy(msg.linear)
angular = sub8_utils.rosmsg_to_numpy(msg.angular)
self.target_position += self.target_orientation.dot(
self._position_gain * linear)
gained_angular = self._orientation_gain * angular
skewed = sub8_utils.skew_symmetric_cross(gained_angular)
rotation = linalg.expm(skewed)
# TODO: Better
# self.target_orientation = self.cur_orientation
# self.target_orientation = rotation.dot(self.target_orientation)
self.target_orientation = self.target_orientation.dot(rotation)
blank = np.eye(4)
blank[:3, :3] = self.target_orientation
self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(
blank)
self.publish_target_pose(self.target_position,
self.target_orientation_quaternion)
def set_pose_server(self, srv):
'''Set the pose of the submarine
TODO: Make this an 'abstract' method of Entity, and assign each new Entity a name/id
'''
rospy.logwarn("Manually setting position of simulated vehicle")
position = sub8_utils.rosmsg_to_numpy(srv.pose.position)
self.body.setPosition(position)
rotation_q = sub8_utils.rosmsg_to_numpy(srv.pose.orientation)
rotation_norm = np.linalg.norm(rotation_q)
velocity = sub8_utils.rosmsg_to_numpy(srv.twist.linear)
angular = sub8_utils.rosmsg_to_numpy(srv.twist.angular)
self.body.setLinearVel(velocity)
self.body.setAngularVel(angular)
# If the commanded rotation is valid
if np.isclose(rotation_norm, 1., atol=1e-2):
self.body.setQuaternion(sub8_utils.normalize(rotation_q))
else:
rospy.logwarn("Commanded quaternion was not a unit quaternion, NOT setting rotation")
return SimSetPoseResponse()
def run(sub):
yield txros.util.wall_sleep(1.0)
# == MISSION CODE HERE ===================
ret = yield missions.align_channel.run(sub)
if ret is None:
print "F**k"
odom = yield sub.last_pose()
rotation = rosmsg_to_numpy(odom.pose.pose.orientation)
# Buoys are around 2m from the ground?
#yield sub.to_height(2)
yield missions.buoy.run(sub)
yield sub.move.backward(1).go()
yield sub.move.up(1.5).go()
yield sub.move.set_orientation(rotation).zero_roll_and_pitch().go()
yield sub.move.forward(5)
print "DONE"
def from_Pose(cls, frame_id, msg):
return cls(frame_id, rosmsg_to_numpy(msg.position), rosmsg_to_numpy(msg.orientation))
def from_Pose(cls, frame_id, msg):
return cls(frame_id, rosmsg_to_numpy(msg.position), rosmsg_to_numpy(msg.orientation))
def test_rosmsg_to_numpy_vector(self):
'''Test a rosmsg conversion for a geometry_msgs/Vector'''
v = Vector3(x=0.1, y=99., z=21.)
numpy_array = rosmsg_to_numpy(v)
np.testing.assert_allclose(np.array([0.1, 99., 21.]), numpy_array)
def test_rosmsg_to_numpy_custom(self):
'''Test a rosmsg conversion for a custom msg'''
pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14)
numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta'])
np.testing.assert_allclose(np.array([1.0, 2.0, 3.14]), numpy_array)
