def cb(self, move_base_goal):
move_base_goal = self.tf.transformPose('map',
move_base_goal.target_pose)
goal_js = [
move_base_goal.pose.position.x, move_base_goal.pose.position.y,
rotation_from_matrix(
quaternion_matrix([
move_base_goal.pose.orientation.x,
move_base_goal.pose.orientation.y,
move_base_goal.pose.orientation.z,
move_base_goal.pose.orientation.w
]))[0]
]
current_js = rospy.wait_for_message('whole_body_controller/state',
JointTrajectoryControllerState)
goal = FollowJointTrajectoryGoal()
goal.trajectory.header = move_base_goal.header
goal.trajectory.joint_names = current_js.joint_names
p = JointTrajectoryPoint()
p.time_from_start = rospy.Duration(0.1)
p.positions = current_js.actual.positions
goal.trajectory.points.append(p)
last_p = p
def next_p(goal_p, last_p, f):
diff = (goal_p - last_p)
if diff > 0:
diff = min(diff, self.max_vel * f)
else:
diff = max(diff, -self.max_vel * f)
return last_p + (diff)
while not np.allclose(np.array(goal_js), p.positions[:3]):
p = JointTrajectoryPoint()
p.time_from_start = last_p.time_from_start + rospy.Duration(
1 * self.freq)
p.positions = list(current_js.actual.positions)
p.positions[0] = next_p(goal_js[0], last_p.positions[0], self.freq)
p.positions[1] = next_p(goal_js[1], last_p.positions[1], self.freq)
p.positions[2] = next_p(goal_js[2], last_p.positions[2], self.freq)
goal.trajectory.points.append(p)
last_p = p
self.client.send_goal(goal)
while not self.client.wait_for_result(rospy.Duration(.1)):
if self.server.is_preempt_requested():
rospy.loginfo('new goal, cancel old one')
self.client.cancel_all_goals()
self.server.set_preempted(MoveBaseResult())
break
# result = self.client.get_result()
# if result.error_code == result.SUCCESSFUL:
self.server.set_succeeded(MoveBaseResult())
def move_base_cb(self, goal):
pwcs = PoseWithCovarianceStamped()
pwcs.header = goal.target_pose.header
pwcs.pose.pose = goal.target_pose.pose
if self.move_base_mode == "beam":
rospy.loginfo("move_base: beaming robot to new goal")
self.initalpose_callback(pwcs)
elif self.move_base_mode == "linear_nav":
move_base_linear_action_name = "/move_base_linear"
ac_move_base_linear = actionlib.SimpleActionClient(move_base_linear_action_name, MoveBaseAction)
rospy.loginfo("Waiting for ActionServer: %s", move_base_linear_action_name)
if not ac_move_base_linear.wait_for_server(rospy.Duration(1)):
rospy.logerr("Emulator move_base failed because move_base_linear action server is not available")
self.as_move_base.set_aborted()
return
rospy.loginfo("send goal to %s", move_base_linear_action_name)
ac_move_base_linear.send_goal(goal)
ac_move_base_linear.wait_for_result()
ac_move_base_linear_status = ac_move_base_linear.get_state()
ac_move_base_linear_result = ac_move_base_linear.get_result()
if ac_move_base_linear_status != GoalStatus.SUCCEEDED:
rospy.logerr("Emulator move_base failed because move_base_linear failed")
self.as_move_base.set_aborted()
return
else:
rospy.logerr("Invalid move_base_action_mode")
self.as_move_base.set_aborted()
return
rospy.loginfo("Emulator move_base succeeded")
self.as_move_base.set_succeeded(MoveBaseResult())
def test_happy_flow(self):
"""
The robot is told to go charge.
It should then navigate to a position close to the charger,
call the dock-service then uses it's arm to plug itself in and say something when
finally getting some juice from the charger.
"""
self.human_to_robot_speech.publish('Go charge')
nav_goal = (10, 10, 0) # Just a random goal in x, y and orientation
move_base_goal = self.move_base.await_goal(
marker='start_move_to_charger')
navigated_to_goal = any(
self.move_base.match_in_received_goals(
[nav_goal], key=self.move_base.goal_to_xy_theta))
already_at_goal = self.move_base.pose_bl == nav_goal
assert navigated_to_goal or already_at_goal, "Robot is not at goal for service"
self.move_base.direct_reply(MoveBaseResult(),
marker='end_move_to_charger')
self.dock.reply(SetStringResponse(success=True),
marker='dock_into_charger')
self.arm.reply(FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.SUCCESSFUL),
marker='arm_plug')
while not self.say.match_in_received_goals(
["Ah, some juice!", "Jummy, fresh juice!"],
key=lambda req: req.text) and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.loginfo("test_happy_flow succeeded")
class W2WMissionPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def __init__(self, name):
self._action_name = name
#self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.planner_as_name = rospy.get_param('~path_planner_as')
self.path_topic = rospy.get_param('~path_topic')
self.wp_topic = rospy.get_param('~wp_topic')
self.map_frame = rospy.get_param('~map_frame', 'map')
# The waypoints as a path
rospy.Subscriber(self.path_topic, Path, self.path_cb, queue_size=1)
self.latest_path = Path()
# LC waypoints, individually
rospy.Subscriber(self.wp_topic, PoseStamped, self.wp_cb, queue_size=1)
# The client to send each wp to the server
self.ac = actionlib.SimpleActionClient(self.planner_as_name,
MoveBaseAction)
while not self.ac.wait_for_server(
rospy.Duration(1)) and not rospy.is_shutdown():
rospy.loginfo("Waiting for action client: %s",
self.planner_as_name)
rospy.loginfo("Action client connected: %s", self.planner_as_name)
while not rospy.is_shutdown():
if self.latest_path.poses:
# Get next waypoint in path
rospy.loginfo("Sending WP")
wp = self.latest_path.poses[0]
del self.latest_path.poses[0]
# TODO: normalize quaternions here according to rviz warning?
goal = MoveBaseGoal(wp)
goal.target_pose.header.frame_id = self.map_frame
self.ac.send_goal(goal)
self.ac.wait_for_result()
rospy.loginfo("WP reached, moving on to next one")
else:
rospy.Rate(1).sleep()
def path_cb(self, path_msg):
self.latest_path = path_msg
rospy.loginfo("Path received with number of wp: %d",
len(self.latest_path.poses))
def wp_cb(self, wp_msg):
# Waypoints for LC from the backseat driver
rospy.loginfo("LC wp received")
self.latest_path.poses.insert(0, wp_msg)
def execute(self, goal):
rospy.loginfo("Bug algorithm goal: x:{} y:{}".format(goal.target_pose.pose.position.x, goal.target_pose.pose.position.x))
rate = rospy.Rate(self.controller_freq)
waypoint = goal.target_pose
dist, yaw_error = self.waypoint_info(waypoint)
# Calculate timeout
start_time = rospy.Time.now().to_sec()
timeout = rospy.Duration(dist / 0.3 * 1.2 + 3).to_sec()
rospy.loginfo("dist: {}, now: {}, timeout: {}, dt: {}".format(dist, start_time, timeout, rospy.Duration(dist / 0.3 * 1.1 + 3).to_sec()))
while dist > self.distance_tolerance:
# If preemt
if self.server.is_preempt_requested():
rospy.loginfo("PREEMPT bug_algorithm")
self.server.set_preempted()
return
# If timeout
if rospy.Time.now().to_sec() - start_time > timeout:
rospy.loginfo("ABORTED bug algorithm")
self.server.set_aborted()
return
try:
# Calculate waypoint data
dist, yaw_error = self.waypoint_info(waypoint)
# print("dist: {:.3f} yaw_err:{:.2f}".format(dist, yaw_error))
# Switch states
if dist < self.distance_tolerance:
break # Waypoint reached
# Sum sonar and bump sensor vectors
vel_x, ang_z = self.get_range_vector_sum()
# Return to heading P controller
ang_z -= clamp(0.8 * yaw_error, -self.max_heading_fix, self.max_heading_fix)
# Publish cmd_vel
# self.cmd_vel_msg.linear.x = (vel_x + self.rc_speed) * self.power_coef # Testing
self.cmd_vel_msg.linear.x = (vel_x + 0.35) * self.power_coef # Event
self.cmd_vel_msg.angular.z = ang_z * self.power_coef
self.cmd_vel_msg.linear.x = clamp(self.cmd_vel_msg.linear.x, -self.max_vel_x, self.max_vel_x)
self.cmd_vel_msg.angular.z = clamp(self.cmd_vel_msg.angular.z, -self.max_ang_z, self.max_ang_z)
self.cmd_vel_pub.publish(self.cmd_vel_msg)
# print("PUB: {:.3f}, {:.3f}".format(self.cmd_vel_msg.linear.x, self.cmd_vel_msg.angular.z))
except Exception as e:
rospy.logerr("HOPE NO CRASH: {}".format(e))
rate.sleep()
rospy.loginfo("Done bug algorithm")
self.server.set_succeeded(result=MoveBaseResult())
def test_unhappy_flow(self):
"""
The robot is told to go charge.
It should then navigate to a position close to the charger.
call the dock-service then uses it's arm to plug itself in and say something when
finally getting some juice from the charger.
However, the docking fails first so the robot retries and then the arm fails at first,
so the robot says something about it's arm and then retries
"""
self.human_to_robot_speech.publish('Go charge')
nav_goal = (10, 10, 0) # Just a random goal in x, y and orientation
move_base_goal = self.move_base.await_goal()
navigated_to_goal = any(
self.move_base.match_in_received_goals(
[nav_goal], key=self.move_base.goal_to_xy_theta))
already_at_goal = self.move_base.pose_bl == nav_goal
assert navigated_to_goal or already_at_goal, "Robot is not at goal for service"
self.move_base.direct_reply(MoveBaseResult())
# Using a custom reply, we can override the default reply set on the dock service temporarily
with self.dock.custom_reply():
self.dock.reply(SetStringResponse(success=False))
# the robot notices docking has failed, so it should say something about that
say_goal = self.say.await_goal()
assert 'fail' in say_goal.text
self.say.direct_reply(SayResult(success=True))
# Then retry docking, now successfully
self.dock.reply(SetStringResponse(success=True))
with self.arm.custom_reply():
self.arm.reply(
FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.
PATH_TOLERANCE_VIOLATED))
# the robot notices docking has failed, so it should say something about that
say_goal = self.say.await_goal()
assert 'fail' in say_goal.text
self.say.direct_reply(SayResult(success=True))
self.arm.reply(
FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.SUCCESSFUL))
while not self.say.match_in_received_goals(
["Ah, some juice!", "Jummy, fresh juice!"],
key=lambda req: req.text) and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.loginfo("test_unhappy_flow succeeded")
def cb(self, move_base_goal):
"""
Callback for nav_p action server
:param move_base_goal: goal position for the nav_p controller
:type move_base_goal: MoveBaseGoal
"""
move_base_goal = self.tf.transform_pose(self.odom_frame,
move_base_goal.target_pose)
self.tf.set_frame_from_pose(self.robot_root, move_base_goal)
self.server.set_succeeded(MoveBaseResult())
def execute_cb(self, goal):
rospy.loginfo("Emergency action initiated")
r = rospy.Rate(11.) # 10hz
while not rospy.is_shutdown():
# Preempted
if self._as.is_preempt_requested():
# Publish emergency command
self.emergency_pub.publish(False)
#Disable controllers
self.lcg_pid_enable.publish(True)
self.vbs_pid_enable.publish(True)
self.tcg_pid_enable.publish(True)
self.yaw_pid_enable.publish(True)
self.depth_pid_enable.publish(True)
self.vel_pid_enable.publish(True)
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted(MoveBaseResult(),
"Preempted EmergencySurface action")
return
# Publish emergency command
self.emergency_pub.publish(True)
#Disable controllers
self.lcg_pid_enable.publish(False)
self.vbs_pid_enable.publish(False)
self.tcg_pid_enable.publish(False)
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
#set VBS to 0
vbs_level = PercentStamped()
vbs_level.value = 0.0
self.vbs_pub.publish(vbs_level)
# Stop thrusters
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = 0.0
rpm.thruster_2_rpm = 0.0
self.rpm_pub.publish(rpm)
r.sleep()
rospy.loginfo('%s: Completed' % self._action_name)
def move_base_cb(self, goal):
rospy.loginfo('move_base_cb')
self.target_position = goal.target_pose.pose.position
self.target_orientation = goal.target_pose.pose.orientation
self.moves_base = True
while not self.reply:
rospy.sleep(0.2)
(trans, rot) = self.listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
self.robot_pose_.pose.position.x = trans[0]
self.robot_pose_.pose.position.y = trans[1]
feedback = MoveBaseFeedback()
feedback.base_position.pose.position.x = \
self.robot_pose_.pose.position.x
feedback.base_position.pose.position.y = \
self.robot_pose_.pose.position.y
self.move_base_as_.publish_feedback(feedback)
if self.move_base_as_.is_preempt_requested():
self.preempted += 1
rospy.loginfo("Preempted!")
self.moves_base = False
self.move_base_as_.set_preempted()
return None
if self.move_base_as_.is_new_goal_available():
self.preempted += 1
self.move_base_cb(self.move_base_as_.accept_new_goal())
return None
else:
result = MoveBaseResult()
self.reply = False
self.preempted = 0
self.moves_base = False
self.target_position = Point()
self.target_orientation = Quaternion()
if self.navigation_succedes:
self.move_base_as_.set_succeeded(result)
else:
self.move_base_as_.set_aborted(result)
def move_base_cb(self, req):
self.base_client.set_succeeded(MoveBaseResult(), "Goal reached.")
class BackseatDriver(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def __init__(self, name):
self._action_name = name
#self.heading_offset = rospy.get_param('~heading_offsets', 5.)
# self.planner_as_name = rospy.get_param('~path_planner_as')
self.path_topic = rospy.get_param('~path_topic')
self.map_frame = rospy.get_param('~map_frame', 'map')
self.base_frame = rospy.get_param('~base_frame', 'base_link')
self.avg_pose_top = rospy.get_param("~average_pose_topic",
'/average_pose')
self.cov_threshold = rospy.get_param("~cov_threshold", 50)
self.wp_topic = rospy.get_param('~wp_topic')
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
# To send LC wp to the mission planner
self.wp_pub = rospy.Publisher(self.wp_topic, PoseStamped, queue_size=1)
self.listener = tf.TransformListener()
# The waypoints as a path
rospy.Subscriber(self.path_topic, Path, self.path_cb, queue_size=1)
self.latest_path = Path()
# The PF filter state
rospy.Subscriber(self.avg_pose_top,
PoseWithCovarianceStamped,
self.pf_cb,
queue_size=1)
self.closing_loop = False
self.new_wp = PoseStamped()
# The LC waypoints, as a path
self.lc_waypoints = Path()
self.lc_waypoints.header.frame_id = self.map_frame
# One LC wp for testing
lc_wp = PoseStamped()
lc_wp.header.frame_id = self.map_frame
lc_wp.pose.position.x = -183.65
lc_wp.pose.position.y = -332.39
lc_wp.pose.position.z = 0.
lc_wp.pose.orientation.w = 1
self.lc_waypoints.poses.append(lc_wp)
rospy.spin()
def path_cb(self, path_msg):
self.latest_path = path_msg
rospy.loginfo("Path received with number of wp: %d",
len(self.latest_path.poses))
def pf_cb(self, pf_msg):
# Reconstruct covariance
self.cov = np.zeros((6, 6))
for i in range(3):
for j in range(3):
self.cov[i, j] = pf_msg.pose.covariance[i * 3 + j]
# Reconstruct pose estimate
position_estimate = pf_msg.pose.pose.position
# Monitor trace
trc = np.sum(np.diag(self.cov))
if trc > self.cov_threshold and not self.closing_loop:
# Pose uncertainty too high, closing the loop to relocalize
# Find LC wp between current PF pose and next wp on the survey
self.new_wp = self.lc_waypoints.poses[0]
# Preempt current waypoint/path
self.wp_pub.publish(self.new_wp)
rospy.loginfo("Sent LC waypoint")
self.closing_loop = True
elif self.closing_loop:
rospy.loginfo("Going for a loop closure!")
try:
(trans,
rot) = self.listener.lookupTransform(self.map_frame,
self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
start_pos = np.array(trans)
end_pos = np.array([
self.new_wp.pose.position.x, self.new_wp.pose.position.y,
self.new_wp.pose.position.z
])
rospy.loginfo("BS driver diff " +
str(np.linalg.norm(start_pos - end_pos)))
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
# Goal reached
rospy.loginfo("Loop closed!")
self.closing_loop = False
def execute_callback(self, move_base_goal):
self.goal = move_base_goal.target_pose.pose # Set the provided goal as the current goal
rospy.logdebug('[Move Base Fake] Execute Callback: {}'.format(
str(self.goal.position)))
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
r = rospy.Rate(1)
while not rospy.is_shutdown():
# Always start by checking if there is a new goal that preempts the current one
if self.action_server.is_preempt_requested():
# Tell Bug algorithm to stop before changing or stopping goal
self.pub_state_to_bug(False,
self.bugType) # Stop bug algorithm
if self.action_server.is_new_goal_available():
new_goal = self.action_server.accept_new_goal(
) # There is a new goal
rospy.logdebug('[Move Base Fake] New Goal: {}'.format(
str(self.goal.position)))
self.goal = new_goal.target_pose.pose # Set the provided goal as the current goal
self.valid_goal = True # Assume it is valid
self.current_goal_started = False # It hasnt started yet
self.current_goal_complete = False # It hasnt been completed
else:
self.action_server.set_preempted(
) # No new goal, we've just been told to stop
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
return
# Start goal
if self.valid_goal and not self.current_goal_started:
rospy.logdebug('[Move Base Fake] Starting Goal')
#set the goal
self.pub_goal_to_bug(self.goal, self.bugType)
#start to go to the goal
self.pub_state_to_bug(True, self.bugType)
self.current_goal_started = True # Only start once
# Feedback ever loop just reports current location
feedback = MoveBaseFeedback()
feedback.base_position.pose.position = self.position
self.action_server.publish_feedback(feedback)
# Completed is set in a callback that you need to link to a service or subscriber
if self.current_goal_complete:
rospy.logdebug('[Move Base Fake] Finishing Goal')
#stop the bug algorithm
self.pub_state_to_bug(False, self.bugType)
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
self.action_server.set_succeeded(
MoveBaseResult(), 'Goal reached') # Send success message
return
r.sleep()
# Shutdown
rospy.logdebug('[Move Base Fake] Shutting Down')
self.pub_state_to_bug(False, self.bugType)
self.goal = None # Stop everything
self.valid_goal = False
self.current_goal_started = False
self.current_goal_complete = False
def execute_cb(goal):
rospy.loginfo("Received a goal")
# check for network cable
if network_connected:
server.set_aborted(
None, "Dangerous to navigate with network cable connected.")
return
# check for power cable
if ac_power_connected:
server.set_aborted(
None, "Dangerous to navigate with AC power cable connected.")
return
# check for power cable
# TODO
# start the arms tucking
tuck_arms_client = actionlib.SimpleActionClient('tuck_arms',
TuckArmsAction)
tuck_arms_client.wait_for_server()
tuck_goal = TuckArmsGoal()
tuck_goal.tuck_left = True
tuck_goal.tuck_right = True
tuck_arms_client.send_goal(tuck_goal)
# start the torso lowering
torso_client = actionlib.SimpleActionClient(
'torso_controller/position_joint_action', SingleJointPositionAction)
torso_client.wait_for_server()
torso_down_goal = SingleJointPositionGoal()
torso_down_goal.position = 0
torso_client.send_goal(torso_down_goal)
# configure the tilting laser
#if not set_tilt_profile([1.05, -.7, 1.05], [0.0, 1.8, 2.0125 + .3]):
if not set_tilt_profile([0.1, 0.1, 0.1], [0.0, 2, 4]):
server.set_aborted(MoveBaseResult(),
"Couldn't set the profile on the laser")
return
configure_laser()
configure_head()
# wait for tuck_arms to finish. (Don't wait for torso to lower, that is a Nice To Have)
while not tuck_arms_client.wait_for_result(rospy.Duration(0.1)):
if server.is_preempt_requested():
if not server.is_new_goal_available():
tuck_arms_client.cancel_goal()
tuck_state = tuck_arms_client.get_state()
if tuck_state != GoalStatus.SUCCEEDED:
if tuck_state == GoalStatus.PREEMPTED:
server.set_preempted(MoveBaseResult(), "Tuck-arms preempted")
elif tuck_state == GoalStatus.ABORTED:
server.set_aborted(MoveBaseResult(), "Tuck-arms aborted")
return
# Now everything should be ready so send the navigation goal.
move_base_client.send_goal(goal, None, None, feedback_cb)
while not move_base_client.wait_for_result(rospy.Duration(0.1)):
#in the unlikely event of network cable being plugged in while moving, stop moving.
if network_connected:
move_base_client.cancel_goal()
server.set_aborted(
None, "Dangerous to navigate with network cable connected.")
return
#in the unlikely event of ac power cable being plugged in while moving, stop moving.
if ac_power_connected:
move_base_client.cancel_goal()
server.set_aborted(
None, "Dangerous to navigate with ac power cable connected.")
return
if server.is_preempt_requested():
if not server.is_new_goal_available():
rospy.loginfo(
"Preempt requested without new goal, cancelling move_base goal."
)
move_base_client.cancel_goal()
server.set_preempted(MoveBaseResult(),
"Got preempted by a new goal")
return
terminal_state = move_base_client.get_state()
result = move_base_client.get_result()
if terminal_state == GoalStatus.PREEMPTED:
server.set_preempted(result)
elif terminal_state == GoalStatus.SUCCEEDED:
server.set_succeeded(result)
elif terminal_state == GoalStatus.ABORTED:
server.set_aborted(result)
else:
server.set_aborted(result, "Unknown result from move_base")
def do_move_base(goal):
rospy.sleep(2)
result = MoveBaseResult()
server.set_succeeded(result)
class P2PPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
# helper variables
#r = rospy.Rate(1)
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
r = rospy.Rate(10.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
self.nav_goal = None
# Stop thrusters
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = 0.
self.rpm_pub.publish(rpm)
break
# Publish feedback
if counter % 100 == 0:
try:
(trans, rot) = self.listener.lookupTransform(
"/world", "sam/base_link", rospy.Time(0))
pose_fb = PoseStamped()
pose_fb.header.frame_id = "/world"
pose_fb.pose.position.x = trans[0]
pose_fb.pose.position.y = trans[1]
pose_fb.pose.position.z = trans[2]
self._feedback.base_position = pose_fb
self._feedback.base_position.header.stamp = rospy.get_rostime(
)
self._as.publish_feedback(self._feedback)
rospy.loginfo("Sending feedback")
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("Error with tf")
continue
# Thruster forward
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = 1000.
self.rpm_pub.publish(rpm)
rospy.loginfo("Thrusters forward")
counter += 1
r.sleep()
if success:
# Stop thruster
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = -1000.0
cnt = 0
while not rospy.is_shutdown() and cnt < 50:
self.rpm_pub.publish(rpm)
cnt += 1
r.sleep()
#self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def timer_callback(self, event):
if self.nav_goal is None:
rospy.loginfo("Nav goal is None!")
return
try:
(trans,
rot) = self.listener.lookupTransform("/world", self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
# TODO: we could use this code for the other check also
goal_point = PointStamped()
goal_point.header.frame_id = "/world"
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_base = self.listener.transformPoint(
self.base_frame, goal_point)
if goal_point_base.point.x < 0.:
rospy.loginfo("Ahead of goal, returning success!")
self.nav_goal = None
return
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
#print("Checking if nav goal is reached!")
current_pos = np.array(trans)
current_pos[2] = 0.0
end_pos = np.array(
[self.nav_goal.position.x, self.nav_goal.position.y, 0.])
if np.linalg.norm(current_pos - end_pos) < self.goal_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
#else:
# print("Did not reach nav goal!")
def __init__(self, name):
"""Plot an example bezier curve."""
self._action_name = name
self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.base_frame = rospy.get_param('~base_frame',
"lolo_auv_1/base_link")
self.nav_goal = None
self.listener = tf.TransformListener()
rospy.Timer(rospy.Duration(2), self.timer_callback)
self.rpm_pub = rospy.Publisher('/uavcan_rpm_command',
ThrusterRPMs,
queue_size=10)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
r = rospy.Rate(10) # 10hz
rospy.spin()
class WPDepthPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def create_marker(self, yaw_setpoint, depth_setpoint):
self.marker.header.frame_id = "/sam/odom"
self.marker.header.stamp = rospy.Time(0)
self.marker.ns = "/sam/viz"
self.marker.id = 0
self.marker.type = 0
self.marker.action = 0
self.marker.pose.position.x = self._feedback.base_position.pose.position.x
self.marker.pose.position.y = self._feedback.base_position.pose.position.y
self.marker.pose.position.z = self._feedback.base_position.pose.position.z
q = quaternion_from_euler(0,0,yaw_setpoint)
self.marker.pose.orientation.x = q[0]
self.marker.pose.orientation.y = q[1]
self.marker.pose.orientation.z = q[2]
self.marker.pose.orientation.w = q[3]
self.marker.scale.x = 1
self.marker.scale.y = 0.1
self.marker.scale.z = 0.1
self.marker.color.a = 1.0 # Dont forget to set the alpha!
self.marker.color.r = 1.0
self.marker.color.g = 1.0
self.marker.color.b = 1.0
self.marker_pub.publish(self.marker)
def yaw_feedback_cb(self,yaw_feedback):
self.yaw_feedback= yaw_feedback.data
def angle_wrap(self,angle):
if(abs(angle)>3.141516):
angle= angle - (abs(angle)/angle)*2*3.141516; #Angle wrapping between -pi and pi
rospy.loginfo_throttle_identical(20, "Angle Error Wrapped")
return angle
def turbo_turn(self,angle_error):
rpm = self.turbo_turn_rpm
rudder_angle = self.rudder_angle
flip_rate = self.flip_rate
left_turn = True
#left turn increases value of yaw angle towards pi, right turn decreases it towards -pi.
if angle_error < 0:
left_turn = False
rospy.loginfo('Right turn!')
rospy.loginfo('Turbo Turning!')
if left_turn:
rudder_angle = -rudder_angle
thrust_rate = 11.
rate = rospy.Rate(thrust_rate)
self.vec_pub.publish(0., rudder_angle, Header())
loop_time = 0.
dualrpm = DualThrusterRPM()
while not rospy.is_shutdown() and loop_time < .37/flip_rate:
dualrpm.thruster_front.rpm = rpm
dualrpm.thruster_back.rpm = rpm
self.rpm_pub.publish(dualrpm)
loop_time += 1./thrust_rate
rate.sleep()
self.vec_pub.publish(0., -rudder_angle, Header())
loop_time = 0.
while not rospy.is_shutdown() and loop_time < .63/flip_rate:
dualrpm.thruster_front.rpm = -rpm
dualrpm.thruster_back.rpm = -rpm
self.rpm_pub.publish(dualrpm)
loop_time += 1./thrust_rate
rate.sleep()
def execute_cb(self, goal):
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
self.nav_goal_frame = goal.target_pose.header.frame_id
if self.nav_goal_frame is None or self.nav_goal_frame == '':
rospy.logwarn("Goal has no frame id! Using utm by default")
self.nav_goal_frame = 'utm'
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(self.nav_goal_frame, goal_point)
self.nav_goal.position.x = goal_point_local.point.x
self.nav_goal.position.y = goal_point_local.point.y
self.nav_goal.position.z = goal_point_local.point.z
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print ("Not transforming point to world local")
pass
rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x)
r = rospy.Rate(11.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
self.yaw_pid_enable.publish(True)
self.depth_pid_enable.publish(True)
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
success = False
self.nav_goal = None
# Stop thrusters
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = 0.
rpm.thruster_back.rpm = 0.
self.rpm_pub.publish(rpm)
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
print('wp depth action planner: stopped thrusters')
self._as.set_preempted(self._result, "Preempted WP action")
return
# Publish feedback
if counter % 5 == 0:
try:
(trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("Error with tf:"+str(self.nav_goal_frame) + " to "+str(self.base_frame))
continue
pose_fb = PoseStamped()
pose_fb.header.frame_id = self.nav_goal_frame
pose_fb.pose.position.x = trans[0]
pose_fb.pose.position.y = trans[1]
pose_fb.pose.position.z = trans[2]
self._feedback.base_position = pose_fb
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._as.publish_feedback(self._feedback)
#rospy.loginfo("Sending feedback")
#Compute yaw setpoint.
xdiff = self.nav_goal.position.x - pose_fb.pose.position.x
ydiff = self.nav_goal.position.y - pose_fb.pose.position.y
yaw_setpoint = math.atan2(ydiff,xdiff)
print('xdiff:',xdiff,'ydiff:',ydiff,'yaw_setpoint:',yaw_setpoint)
#compute yaw_error (e.g. for turbo_turn)
yaw_error= -(self.yaw_feedback - yaw_setpoint)
yaw_error= self.angle_wrap(yaw_error) #wrap angle error between -pi and pi
depth_setpoint = self.nav_goal.position.z
self.depth_pub.publish(depth_setpoint)
#self.vbs_pid_enable.publish(False)
#self.vbs_pub.publish(depth_setpoint)
if self.vel_ctrl_flag:
rospy.loginfo_throttle_identical(5, "vel ctrl, no turbo turn")
#with Velocity control
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
# Publish to velocity controller
self.vel_pid_enable.publish(True)
self.vel_pub.publish(self.vel_setpoint)
self.roll_pub.publish(self.roll_setpoint)
#rospy.loginfo("Velocity published")
else:
if self.turbo_turn_flag:
#if turbo turn is included
rospy.loginfo("Yaw error: %f", yaw_error)
if abs(yaw_error) > self.turbo_angle_min and abs(yaw_error) < self.turbo_angle_max:
#turbo turn with large deviations, maximum deviation is 3.0 radians to prevent problems with discontinuities at +/-pi
self.yaw_pid_enable.publish(False)
self.turbo_turn(yaw_error)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(True)
self.vbs_pub.publish(depth_setpoint)
else:
rospy.loginfo_throttle_identical(5,"Normal WP following")
#normal turning if the deviation is small
self.vbs_pid_enable.publish(False)
self.depth_pid_enable.publish(True)
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
self.create_marker(yaw_setpoint,depth_setpoint)
# Thruster forward
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = self.forward_rpm
rpm.thruster_back.rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
else:
#turbo turn not included, no velocity control
rospy.loginfo_throttle_identical(5, "Normal WP following, no turbo turn")
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
self.create_marker(yaw_setpoint,depth_setpoint)
# Thruster forward
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = self.forward_rpm
rpm.thruster_back.rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
counter += 1
r.sleep()
# Stop thruster
self.vel_pid_enable.publish(False)
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = 0.0
rpm.thruster_back.rpm = 0.0
self.rpm_pub.publish(rpm)
#Stop controllers
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def timer_callback(self, event):
if self.nav_goal is None:
#rospy.loginfo_throttle(30, "Nav goal is None!")
return
try:
(trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
# TODO: we could use this code for the other check also
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
#print("Checking if nav goal is reached!")
start_pos = np.array(trans)
end_pos = np.array([self.nav_goal.position.x, self.nav_goal.position.y, self.nav_goal.position.z])
# We check for success out of the main control loop in case the main control loop is
# running at 300Hz or sth. like that. We dont need to check succes that frequently.
xydiff = start_pos[:2] - end_pos[:2]
zdiff = np.abs(np.abs(start_pos[2]) - np.abs(end_pos[2]))
xydiff_norm = np.linalg.norm(xydiff)
# rospy.logdebug("diff xy:"+ str(xydiff_norm)+' z:' + str(zdiff))
rospy.loginfo("diff xy:"+ str(xydiff_norm)+' z:' + str(zdiff)+ " WP tol:"+ str(self.wp_tolerance)+ "Depth tol:"+str(self.depth_tolerance))
if xydiff_norm < self.wp_tolerance and zdiff < self.depth_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
def __init__(self, name):
"""Publish yaw and depth setpoints based on waypoints"""
self._action_name = name
#self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.wp_tolerance = rospy.get_param('~wp_tolerance', 5.)
self.depth_tolerance = rospy.get_param('~depth_tolerance', 0.5)
self.base_frame = rospy.get_param('~base_frame', "sam/base_link")
rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/thrusters_cmd')
heading_setpoint_topic = rospy.get_param('~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint')
yaw_pid_enable_topic = rospy.get_param('~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable')
depth_setpoint_topic = rospy.get_param('~depth_setpoint_topic', '/sam/ctrl/dynamic_depth/setpoint')
depth_pid_enable_topic = rospy.get_param('~depth_pid_enable_topic', '/sam/ctrl/dynamic_depth/pid_enable')
self.forward_rpm = int(rospy.get_param('~forward_rpm', 1000))
#related to turbo turn
self.turbo_turn_flag = rospy.get_param('~turbo_turn_flag', False)
thrust_vector_cmd_topic = rospy.get_param('~thrust_vector_cmd_topic', '/sam/core/thrust_vector_cmd')
yaw_feedback_topic = rospy.get_param('~yaw_feedback_topic', '/sam/ctrl/yaw_feedback')
self.turbo_angle_min_deg = rospy.get_param('~turbo_angle_min', 90)
self.turbo_angle_min = np.radians(self.turbo_angle_min_deg)
self.turbo_angle_max = 3.0
self.flip_rate = rospy.get_param('~flip_rate', 0.5)
self.rudder_angle = rospy.get_param('~rudder_angle', 0.08)
self.turbo_turn_rpm = rospy.get_param('~turbo_turn_rpm', 1000)
vbs_pid_enable_topic = rospy.get_param('~vbs_pid_enable_topic', '/sam/ctrl/vbs/pid_enable')
vbs_setpoint_topic = rospy.get_param('~vbs_setpoint_topic', '/sam/ctrl/vbs/setpoint')
#related to velocity regulation instead of rpm
self.vel_ctrl_flag = rospy.get_param('~vel_ctrl_flag', False)
self.vel_setpoint = rospy.get_param('~vel_setpoint', 0.5) #velocity setpoint in m/s
self.roll_setpoint = rospy.get_param('~roll_setpoint', 0)
vel_setpoint_topic = rospy.get_param('~vel_setpoint_topic', '/sam/ctrl/dynamic_velocity/u_setpoint')
roll_setpoint_topic = rospy.get_param('~roll_setpoint_topic', '/sam/ctrl/dynamic_velocity/roll_setpoint')
vel_pid_enable_topic = rospy.get_param('~vel_pid_enable_topic', '/sam/ctrl/dynamic_velocity/pid_enable')
self.nav_goal = None
self.listener = tf.TransformListener()
rospy.Timer(rospy.Duration(0.5), self.timer_callback)
self.yaw_feedback=0
rospy.Subscriber(yaw_feedback_topic, Float64, self.yaw_feedback_cb)
self.rpm_pub = rospy.Publisher(rpm_cmd_topic, DualThrusterRPM, queue_size=10)
self.yaw_pub = rospy.Publisher(heading_setpoint_topic, Float64, queue_size=10)
self.depth_pub = rospy.Publisher(depth_setpoint_topic, Float64, queue_size=10)
self.vel_pub = rospy.Publisher(vel_setpoint_topic, Float64, queue_size=10)
self.roll_pub = rospy.Publisher(roll_setpoint_topic, Float64, queue_size=10)
#TODO make proper if it works.
self.vbs_pub = rospy.Publisher(vbs_setpoint_topic, Float64, queue_size=10)
self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic, Bool, queue_size=10)
self.depth_pid_enable = rospy.Publisher(depth_pid_enable_topic, Bool, queue_size=10)
self.vbs_pid_enable = rospy.Publisher(vbs_pid_enable_topic, Bool, queue_size=10)
self.vel_pid_enable = rospy.Publisher(vel_pid_enable_topic, Bool, queue_size=10)
self.vec_pub = rospy.Publisher(thrust_vector_cmd_topic, ThrusterAngles, queue_size=10)
self.marker = Marker()
self.marker_pub = rospy.Publisher('/sam/viz/wp_marker', Marker, queue_size=1)
self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start = False)
self._as.start()
rospy.loginfo("Announced action server with name: %s", self._action_name)
rospy.spin()
class LeaderFollower(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
rospy.loginfo_throttle(5, "Goal received")
success = True
rate = rospy.Rate(11.) # 10hz
counter = 0
while not rospy.is_shutdown():
self.yaw_pid_enable.publish(True)
self.depth_pid_enable.publish(True)
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
success = False
# Stop thrusters
self.rpm.thruster_1_rpm = 0.
self.rpm.thruster_2_rpm = 0.
self.rpm_pub.publish(self.rpm)
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
print('leader_follower_action: stopped thrusters')
self._as.set_preempted(self._result, "Preempted WP action")
return
# Compute and Publish setpoints
if counter % 1 == 0:
# distance check is done in the BT, we will add CBFs here later, which will include
# that distance as a constraint anyways
# if sqrt(rel_trans[0]**2 + rel_trans[1]**2 + rel_trans[2]**2) < self.min_dist:
# break
#Check transform between SAM1 (leader- goal) and SAM2 (follower -self), define a goal point and call the go_to_point() function
self.leader_frame = goal.target_pose.header.frame_id
try:
(follower_trans,
follower_rot) = self.listener.lookupTransform(
self.follower_odom, self.follower_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logwarn_throttle_identical(
5, "Could not get transform between " +
self.leader_frame + " and " + self.follower_frame)
success = False
break
except:
rospy.logwarn_throttle_identical(
5, "Could not do tf lookup for some other reason")
success = False
break
try:
(leader_trans, leader_rot) = self.listener.lookupTransform(
self.follower_odom, self.leader_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logwarn_throttle_identical(
5, "Could not get transform between " +
self.leader_frame + " and " + self.follower_frame)
success = False
break
except:
rospy.logwarn_throttle_identical(
5, "Could not do tf lookup for some other reason")
success = False
break
xdiff = leader_trans[0] - follower_trans[0]
ydiff = leader_trans[1] - follower_trans[1]
zdiff = leader_trans[2] - follower_trans[2]
yaw_setpoint = math.atan2(ydiff, xdiff)
depth_setpoint = -zdiff
rospy.loginfo_throttle(
10, 'yaw_setpoint:' + str(yaw_setpoint) +
' depth_setpoint:' + str(depth_setpoint))
self.depth_pub.publish(depth_setpoint)
if self.vel_ctrl_flag:
rospy.loginfo_throttle_identical(
5, "vel ctrl, no turbo turn")
#with Velocity control
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
# Publish to velocity controller
self.vel_pid_enable.publish(True)
self.vel_pub.publish(self.vel_setpoint)
self.roll_pub.publish(self.roll_setpoint)
#rospy.loginfo("Velocity published")
else:
#turbo turn not included, no velocity control
rospy.loginfo_throttle_identical(
5, "Normal WP following, no turbo turn")
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
# Thruster forward
self.rpm.thruster_1_rpm = self.forward_rpm
self.rpm.thruster_2_rpm = self.forward_rpm
self.rpm_pub.publish(self.rpm)
#rospy.loginfo("Thrusters forward")
counter += 1
rate.sleep()
# Stop thruster
self.vel_pid_enable.publish(False)
self.rpm.thruster_1_rpm = 0.0
self.rpm.thruster_2_rpm = 0.0
self.rpm_pub.publish(self.rpm)
#Stop controllers
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
if self._result:
rospy.loginfo('%s: Succeeded' % self._action_name)
else:
rospy.logwarn_throttle_identical(3,
'%s: Failed' % self._action_name)
self._as.set_succeeded(self._result)
def __init__(self, name):
self.rpm = ThrusterRPMs()
"""Publish yaw and depth setpoints based on waypoints"""
self._action_name = name
self.follower_frame = rospy.get_param('~follower_frame',
'/sam_2/base_link')
self.follower_odom = rospy.get_param('~follower_odom', '/sam_2/odom')
# self.min_dist = rospy.get_param('~min_dist', 5.)
rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/rpm_cmd')
heading_setpoint_topic = rospy.get_param(
'~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint')
yaw_pid_enable_topic = rospy.get_param(
'~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable')
depth_setpoint_topic = rospy.get_param(
'~depth_setpoint_topic', '/sam/ctrl/dynamic_depth/setpoint')
depth_pid_enable_topic = rospy.get_param(
'~depth_pid_enable_topic', '/sam/ctrl/dynamic_depth/pid_enable')
self.forward_rpm = int(rospy.get_param('~forward_rpm', 1000))
#related to velocity regulation instead of rpm
self.vel_ctrl_flag = rospy.get_param('~vel_ctrl_flag', False)
self.vel_setpoint = rospy.get_param('~vel_setpoint',
0.5) #velocity setpoint in m/s
self.roll_setpoint = rospy.get_param('~roll_setpoint', 0)
vel_setpoint_topic = rospy.get_param(
'~vel_setpoint_topic', '/sam/ctrl/dynamic_velocity/u_setpoint')
roll_setpoint_topic = rospy.get_param(
'~roll_setpoint_topic', '/sam/ctrl/dynamic_velocity/roll_setpoint')
vel_pid_enable_topic = rospy.get_param(
'~vel_pid_enable_topic', '/sam/ctrl/dynamic_velocity/pid_enable')
self.listener = tf.TransformListener()
self.rpm_pub = rospy.Publisher(rpm_cmd_topic,
ThrusterRPMs,
queue_size=10)
self.yaw_pub = rospy.Publisher(heading_setpoint_topic,
Float64,
queue_size=10)
self.depth_pub = rospy.Publisher(depth_setpoint_topic,
Float64,
queue_size=10)
self.vel_pub = rospy.Publisher(vel_setpoint_topic,
Float64,
queue_size=10)
self.roll_pub = rospy.Publisher(roll_setpoint_topic,
Float64,
queue_size=10)
self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic,
Bool,
queue_size=10)
self.depth_pid_enable = rospy.Publisher(depth_pid_enable_topic,
Bool,
queue_size=10)
self.vel_pid_enable = rospy.Publisher(vel_pid_enable_topic,
Bool,
queue_size=10)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
rospy.spin()
class W2WPathPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
self.nav_goal_frame = goal.target_pose.header.frame_id
if self.nav_goal_frame is None or self.nav_goal_frame == '':
rospy.logwarn("Goal has no frame id! Using map by default")
self.nav_goal_frame = self.map_frame
r = rospy.Rate(10.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
self.nav_goal = None
# Stop thruster
self.motion_command(0., 0., 0.)
break
# Transform goal map --> base frame
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(
self.base_frame, goal_point)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("Not transforming point to base frame")
pass
#Compute throttle error
# throttle_level = min(self.max_throttle, np.linalg.norm(
# np.array([goal_point_local.point.x + goal_point_local.point.y])))
# Nacho: no real need to adjust the throttle
throttle_level = self.max_throttle
# Compute thrust error
alpha = math.atan2(goal_point_local.point.y,
goal_point_local.point.x)
sign = np.copysign(1, alpha)
yaw_setpoint = sign * min(self.max_thrust, abs(alpha))
# Command velocities
self.motion_command(throttle_level, yaw_setpoint, 0.)
# Publish feedback
if counter % 10 == 0:
self._as.publish_feedback(self._feedback)
counter += 1
r.sleep()
# Stop thruster
self.motion_command(0., 0., 0.)
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def motion_command(self, throttle_level, thruster_angle,
inclination_angle):
incl = Float64()
throttle = Float64()
thrust = Float64()
throttle.data = throttle_level
thrust.data = thruster_angle
incl.data = inclination_angle
self.thruster_pub.publish(thrust)
self.inclination_pub.publish(incl)
self.throttle_pub.publish(throttle)
def timer_callback(self, event):
if self.nav_goal is None:
#rospy.loginfo_throttle(30, "Nav goal is None!")
return
# Check if the goal has been reached
try:
(trans,
rot) = self.listener.lookupTransform(self.nav_goal_frame,
self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
start_pos = np.array(trans)
end_pos = np.array([
self.nav_goal.position.x, self.nav_goal.position.y,
self.nav_goal.position.z
])
rospy.logdebug("diff " + str(np.linalg.norm(start_pos - end_pos)))
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
# Goal reached
self.nav_goal = None
def __init__(self, name):
self._action_name = name
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.max_throttle = rospy.get_param('~max_throttle', 2.)
self.max_thrust = rospy.get_param('~max_thrust', 0.5)
self.map_frame = rospy.get_param('~map_frame', 'map')
self.base_frame = rospy.get_param('~base_frame', 'base_link')
self.odom_frame = rospy.get_param('~odom_frame', 'odom')
self.throttle_top = rospy.get_param('~throttle_cmd', '/throttle')
self.thruster_top = rospy.get_param('~thruster_cmd', '/thruster')
self.inclination_top = rospy.get_param('~inclination_cmd',
'/inclination')
self.as_name = rospy.get_param('~path_planner_as', 'path_planner')
self.nav_goal = None
self.listener = tf.TransformListener()
rospy.Timer(rospy.Duration(2), self.timer_callback)
self.throttle_pub = rospy.Publisher(self.throttle_top,
Float64,
queue_size=1)
self.thruster_pub = rospy.Publisher(self.thruster_top,
Float64,
queue_size=1)
self.inclination_pub = rospy.Publisher(self.inclination_top,
Float64,
queue_size=1)
self._as = actionlib.SimpleActionServer(self.as_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s", self.as_name)
rospy.spin()
class YawPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
self.nav_goal_frame = goal.target_pose.header.frame_id
if self.nav_goal_frame is None or self.nav_goal_frame == '':
rospy.logwarn("Goal has no frame id! Using utm by default")
self.nav_goal_frame = '/utm'
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(
self.nav_goal_frame, goal_point)
self.nav_goal.position.x = goal_point_local.point.x
self.nav_goal.position.y = goal_point_local.point.y
self.nav_goal.position.z = goal_point_local.point.z
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("Not transforming point to world local")
pass
rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x)
r = rospy.Rate(11.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
self.yaw_pid_enable.publish(True)
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
self.nav_goal = None
# Stop thrusters
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = 0.
rpm.thruster_2_rpm = 0.
self.rpm_pub.publish(rpm)
self.yaw_pid_enable.publish(False)
break
# Publish feedback
if counter % 10 == 0:
try:
(trans, rot) = self.listener.lookupTransform(
self.nav_goal_frame, self.base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("Error with tf:" + str(self.nav_goal_frame) +
" to " + str(self.base_frame))
continue
pose_fb = PoseStamped()
pose_fb.header.frame_id = self.nav_goal_frame
pose_fb.pose.position.x = trans[0]
pose_fb.pose.position.y = trans[1]
pose_fb.pose.position.z = trans[2]
self._feedback.base_position = pose_fb
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._as.publish_feedback(self._feedback)
#rospy.loginfo("Sending feedback")
#Compute yaw setpoint.
xdiff = self.nav_goal.position.x - pose_fb.pose.position.x
ydiff = self.nav_goal.position.y - pose_fb.pose.position.y
yaw_setpoint = math.atan2(ydiff, xdiff)
self.yaw_pub.publish(yaw_setpoint)
#rospy.loginfo("Yaw setpoint: %f", yaw_setpoint)
# Thruster forward
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = self.forward_rpm
rpm.thruster_2_rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
counter += 1
r.sleep()
# Stop thruster
rpm = ThrusterRPMs()
rpm.thruster_1_rpm = 0.0
rpm.thruster_2_rpm = 0.0
self.rpm_pub.publish(rpm)
#Stop yaw controller
self.yaw_pid_enable.publish(False)
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def timer_callback(self, event):
if self.nav_goal is None:
#rospy.loginfo_throttle(30, "Nav goal is None!")
return
try:
(trans,
rot) = self.listener.lookupTransform(self.nav_goal_frame,
self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
# TODO: we could use this code for the other check also
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
#print("Checking if nav goal is reached!")
start_pos = np.array(trans)
end_pos = np.array([
self.nav_goal.position.x, self.nav_goal.position.y,
self.nav_goal.position.z
])
rospy.loginfo("diff " + str(np.linalg.norm(start_pos - end_pos)))
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
def __init__(self, name):
"""Publish yaw setpoints based on waypoints"""
self._action_name = name
#self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.base_frame = rospy.get_param('~base_frame', "sam/base_link")
self.utm_frame = rospy.get_param('~utm_frame', "utm")
rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/rpm_cmd')
heading_setpoint_topic = rospy.get_param(
'~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint')
yaw_pid_enable_topic = rospy.get_param(
'~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable')
self.forward_rpm = int(rospy.get_param('~forward_rpm', 700))
self.nav_goal = None
self.listener = tf.TransformListener()
rospy.Timer(rospy.Duration(2), self.timer_callback)
self.rpm_pub = rospy.Publisher(rpm_cmd_topic,
ThrusterRPMs,
queue_size=10)
self.yaw_pub = rospy.Publisher(heading_setpoint_topic,
Float64,
queue_size=10)
self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic,
Bool,
queue_size=10)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
rospy.spin()
def base_cb(self, goal): self.cb_executed = True result = MoveBaseResult() self.as_server.set_succeeded(result)
class BezierPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def execute_cb(self, goal):
success = True
self.nav_goal = goal.target_pose.pose
# Target in word_utm coord, translate to world_local
goal_point = PointStamped()
goal_point.header.frame_id = "/world_utm"
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(
"/world_local", goal_point)
self.nav_goal.position.x = goal_point_local.point.x
self.nav_goal.position.y = goal_point_local.point.y
self.nav_goal.position.z = goal_point_local.point.z
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("Not transforming point to world local")
pass
rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x)
r = rospy.Rate(10.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
self.nav_goal = None
break
if counter % 100 == 0:
path, pose = self.plan()
self.pub.publish(path)
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._feedback.base_position.header.frame_id = "/world_utm"
self._feedback.base_position = pose
self._as.publish_feedback(self._feedback)
counter += 1
r.sleep()
self.pub.publish(Path())
if success:
#self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def plan(self):
try:
(trans,
rot) = self.listener.lookupTransform("/world_local",
self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return Path(), PoseStamped()
start_pos = np.array(trans)
start_rot = np.array(rot)
euler = tf.transformations.euler_from_quaternion(rot)
start_pitch = euler[1] #np.radians(-40.0) # [rad]
start_yaw = euler[2] # np.radians(180.0) # [rad]
end_pos = np.array([
self.nav_goal.position.x, self.nav_goal.position.y,
self.nav_goal.position.z
])
end_rot = [
self.nav_goal.orientation.x, self.nav_goal.orientation.y,
self.nav_goal.orientation.z, self.nav_goal.orientation.w
]
euler = tf.transformations.euler_from_quaternion(end_rot)
end_pitch = euler[1]
end_yaw = euler[2]
curve, control_points = calc_4points_bezier_path(
start_pos,
start_yaw,
start_pitch,
end_pos,
end_yaw,
end_pitch,
self.heading_offset,
n_points=self.n_points)
#derivatives_cp = bezier_derivatives_control_points(control_points, 1)
path = Path()
path.header.frame_id = "/world_local"
path.header.stamp = rospy.get_rostime()
for i in range(0, self.n_points):
pose = PoseStamped()
pose.pose.position.x = curve.T[0][i]
pose.pose.position.y = curve.T[1][i]
pose.pose.position.z = curve.T[2][i]
path.poses.append(pose)
# Revert pose to UTM coordinates for feedback to the BT
pose = PoseStamped()
pose.header.frame_id = "/world_local"
pose.pose.position.x = start_pos[0]
pose.pose.position.y = start_pos[1]
pose.pose.position.z = start_pos[2]
pose.pose.orientation.x = start_rot[0]
pose.pose.orientation.y = start_rot[1]
pose.pose.orientation.z = start_rot[2]
pose.pose.orientation.w = start_rot[3]
print("orientation")
print(start_rot)
try:
pose_local = self.listener.transformPose("/world_utm", pose)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
return path, pose_local
def timer_callback(self, event):
if self.nav_goal is None:
# print("Nav goal is None!")
return
try:
(trans,
rot) = self.listener.lookupTransform("/world", self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
goal_point = PointStamped()
goal_point.header.frame_id = "/world"
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
# try:
# goal_point_base = self.listener.transformPoint(self.base_frame, goal_point)
# if goal_point_base.point.x < 0.:
# rospy.loginfo("Ahead of goal, returning success!")
# self.nav_goal = None
# return
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# pass
#print("Checking if nav goal is reached!")
start_pos = np.array(trans)
end_pos = np.array([
self.nav_goal.position.x, self.nav_goal.position.y,
self.nav_goal.position.z
])
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
#else:
# print("Did not reach nav goal!")
def __init__(self, name):
"""Plot an example bezier curve."""
self._action_name = name
self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.n_points = rospy.get_param('~number_points', 100)
self.base_frame = rospy.get_param('~base_frame', "base_link")
self.nav_goal = None
self.listener = tf.TransformListener()
self.pub = rospy.Publisher('/global_plan', Path, queue_size=10)
rospy.Timer(rospy.Duration(0.1), self.timer_callback)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
r = rospy.Rate(10) # 10hz
counter = 0
while not rospy.is_shutdown():
if counter % 100 == 0 and self.nav_goal is not None:
path, pose = self.plan()
self.pub.publish(path)
r.sleep()
counter += 1
class BezierPlanner(object):
# create messages that are used to publish feedback/result
_feedback = MoveBaseFeedback()
_result = MoveBaseResult()
def callback(self, pose_msg):
self.nav_goal = pose_msg.pose
path, pose = self.plan()
self.pub.publish(path)
def execute_cb(self, goal):
# helper variables
#r = rospy.Rate(1)
success = True
self.nav_goal = goal.target_pose.pose
# append the seeds for the fibonacci sequence
#self._feedback.base_position.header.frame_id = "/world"
# publish info to the console for the user
#rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))
r = rospy.Rate(0.1) # 10hz
while not rospy.is_shutdown() and self.nav_goal is not None:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
self.nav_goal = None
break
path, pose = self.plan()
self.pub.publish(path)
self._feedback.base_position = pose
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._as.publish_feedback(self._feedback)
r.sleep()
self.pub.publish(Path())
if success:
#self._result.sequence = self._feedback.sequence
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def plan(self):
try:
(trans,
rot) = self.listener.lookupTransform("/world", self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return Path(), PoseStamped()
start_pos = np.array(trans)
euler = tf.transformations.euler_from_quaternion(rot)
start_pitch = euler[1] #np.radians(-40.0) # [rad]
start_yaw = euler[2] # np.radians(180.0) # [rad]
end_pos = np.array(
[self.nav_goal.position.x, self.nav_goal.position.y, -85.])
#if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
# rospy.loginfo("Reached goal!")
# self.nav_goal = None
# return Path()
end_rot = [
self.nav_goal.orientation.x, self.nav_goal.orientation.y,
self.nav_goal.orientation.z, self.nav_goal.orientation.w
]
euler = tf.transformations.euler_from_quaternion(end_rot)
end_pitch = euler[1]
end_yaw = euler[2]
curve, control_points = calc_4points_bezier_path(
start_pos,
start_yaw,
start_pitch,
end_pos,
end_yaw,
end_pitch,
self.heading_offset,
n_points=self.n_points)
#derivatives_cp = bezier_derivatives_control_points(control_points, 1)
path = Path()
path.header.frame_id = "/world"
path.header.stamp = rospy.get_rostime()
for i in range(0, self.n_points):
pose = PoseStamped()
pose.pose.position.x = curve.T[0][i]
pose.pose.position.y = curve.T[1][i]
pose.pose.position.z = curve.T[2][i]
path.poses.append(pose)
pose = PoseStamped()
pose.pose.position.x = start_pos[0]
pose.pose.position.y = start_pos[1]
pose.pose.position.z = start_pos[2]
return path, pose
def timer_callback(self, event):
if self.nav_goal is None:
#print("Nav goal is None!")
return
try:
(trans,
rot) = self.listener.lookupTransform("/world", self.base_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return
#print("Checking if nav goal is reached!")
start_pos = np.array(trans)
end_pos = np.array(
[self.nav_goal.position.x, self.nav_goal.position.y, -85.])
if np.linalg.norm(start_pos - end_pos) < self.goal_tolerance:
rospy.loginfo("Reached goal!")
self.nav_goal = None
#else:
# print("Did not reach nav goal!")
def __init__(self, name):
"""Plot an example bezier curve."""
self._action_name = name
self.heading_offset = rospy.get_param('~heading_offsets', 5.)
self.goal_tolerance = rospy.get_param('~goal_tolerance', 5.)
self.n_points = rospy.get_param('~number_points', 100)
self.base_frame = rospy.get_param('~base_frame',
"lolo_auv_1/base_link")
self.nav_goal = None
self.listener = tf.TransformListener()
self.pub = rospy.Publisher('/global_plan', Path, queue_size=10)
rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.callback)
rospy.Timer(rospy.Duration(1), self.timer_callback)
self._as = actionlib.SimpleActionServer(self._action_name,
MoveBaseAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("Announced action server with name: %s",
self._action_name)
r = rospy.Rate(0.1) # 10hz
while not rospy.is_shutdown():
if self.nav_goal is not None:
path, pose = self.plan()
self.pub.publish(path)
r.sleep()
This action server node will wait for 3 MoveBaseGoals and handle each differently:
- succeed the first one
- abort the 2nd goal
- just ignore the 3rd one
"""
import rospy
from scenario_test_tools.scriptable_move_base import ScriptableMoveBase
from move_base_msgs.msg import MoveBaseAction, MoveBaseResult
if __name__ == "__main__":
rospy.init_node('move_base_fake')
succeeded = MoveBaseResult()
# By default, wait 5 secs for the robot to arrive
result_delay = rospy.get_param("~result_delay", 5)
# By default, wait forever for a move_base goal
timeout = rospy.get_param("~timeout", None)
# By default, publish transformation /map to /base_link
pub_transform = rospy.get_param("~pub_transform", True)
move_base = ScriptableMoveBase(rospy.get_name(),
MoveBaseAction,
default_result_delay=result_delay,
pub_transform=pub_transform)
move_base.start()
rospy.loginfo("fake move base running")