def handleSrv(self, req):
if req.start_request:
rospy.loginfo("Received Start Request")
self.isAborted = False
self.defaultDepth = req.start_ctrl.depth_setpoint
self.laneSearchDepth = self.defaultDepth
self.inputHeading = req.start_ctrl.heading_setpoint
self.expectedLanes = req.numLanes
self.chosenLane = self.LEFT if req.chosenLane == 0 else self.RIGHT
rospy.loginfo("Input: d={}, h={}, num={}, chosen={}".format(
self.defaultDepth, self.inputHeading,
self.expectedLanes, self.chosenLane))
return mission_to_visionResponse(start_response=True,
abort_response=False,
data=controller(heading_setpoint=
self.curHeading))
elif req.abort_request:
rospy.loginfo("Received Abort Request")
self.motionClient.cancel_all_goals()
self.isAborted = True
while not self.abortedDone:
rospy.sleep(rospy.Duration(0.3))
self.motionClient.cancel_all_goals()
return mission_to_visionResponse(start_response=False,
abort_response=True,
data=controller(heading_setpoint=
self.curHeading))
def handleSrv(self, req):
if req.start_request:
rospy.loginfo("Received Start Request")
self.isAborted = False
self.defaultDepth = req.start_ctrl.depth_setpoint
self.laneSearchDepth = self.defaultDepth
self.inputHeading = req.start_ctrl.heading_setpoint
self.expectedLanes = req.numLanes
self.chosenLane = self.LEFT if req.chosenLane == 0 else self.RIGHT
rospy.loginfo("Input: d={}, h={}, num={}, chosen={}".format(
self.defaultDepth, self.inputHeading, self.expectedLanes,
self.chosenLane))
return mission_to_visionResponse(
start_response=True,
abort_response=False,
data=controller(heading_setpoint=self.curHeading))
elif req.abort_request:
rospy.loginfo("Received Abort Request")
self.motionClient.cancel_all_goals()
self.isAborted = True
while not self.abortedDone:
rospy.sleep(rospy.Duration(0.3))
self.motionClient.cancel_all_goals()
return mission_to_visionResponse(
start_response=False,
abort_response=True,
data=controller(heading_setpoint=self.curHeading))
def RGBCallback(self, r):
if r.task_complete_request:
self.RGBStatus['done'] = True
return vision_to_missionResponse(search_response=False,
task_complete_ctrl=controller(),
task_complete_response=True)
if r.fail_request:
self.RGBStatus['failed'] = True
return vision_to_missionResponse(search_response=False,
task_complete_ctrl=controller(),
task_complete_response=False)
def RGBCallback(self, r):
if r.task_complete_request:
self.RGBStatus['done'] = True
return vision_to_missionResponse(search_response=False,
task_complete_ctrl=controller(),
task_complete_response=True)
if r.fail_request:
self.RGBStatus['failed'] = True
return vision_to_missionResponse(search_response=False,
task_complete_ctrl=controller(),
task_complete_response=False)
def laneCallback(self, r):
if r.task_complete_request:
rospy.loginfo(str(r))
self.laneStatus['complete'] = True
self.laneStatus['heading'] = r.task_complete_ctrl.heading_setpoint
return vision_to_missionResponse(search_response=True,
task_complete_response=True,
data=controller())
if r.fail_request:
self.laneStatus['aborted'] = True
return vision_to_missionResponse(search_response=False,
task_complete_response=False,
data=controller())
def laneCallback(self, r):
if r.task_complete_request:
rospy.loginfo(str(r))
self.laneStatus['complete'] = True
self.laneStatus['heading'] = r.task_complete_ctrl.heading_setpoint
return vision_to_missionResponse(search_response=True,
task_complete_response=True,
data=controller())
if r.fail_request:
self.laneStatus['aborted'] = True
return vision_to_missionResponse(search_response=False,
task_complete_response=False,
data=controller())
def searchComplete(self):
rospy.loginfo("Sending centered request to mission planner")
if not self.isAlone:
self.toMission(fail_request=False, task_complete_request=False,
centered=True,
task_complete_ctrl=controller(heading_setpoint=
self.curHeading))
def execute(self, userdata):
global mission_srv
laneDetector.frameCallback = None
# Lock on to confirmed heading and go towards it
rospy.loginfo('locking on and going')
rospy.loginfo('Headings: ' + str(userdata.headings))
finalHeading = userdata.headings[0]
if userdata.expectedLanes > 1:
# Assume we only have up to two lanes for now
angleDelta = userdata.headings[1] - userdata.headings[0]
leftIndex = 0 if 0 < angleDelta < 180 or angleDelta < -180 else 1
leftHeading, rightHeading = userdata.headings[leftIndex], userdata.headings[1-leftIndex]
if userdata.outDir == 'left':
finalHeading = leftHeading
elif userdata.outDir == 'right':
finalHeading = rightHeading
rospy.loginfo("Using heading %f" % finalHeading)
ctrl = controller()
ctrl.depth_setpoint = depth_setpoint
ctrl.heading_setpoint = finalHeading
if not TEST_MODE:
mission_srv(search_request=False, task_complete_request=True, task_complete_ctrl=ctrl)
else:
return 'killed'
return 'succeeded'
def execute(self, userdata):
global mission_srv
laneDetector.frameCallback = None
# Lock on to confirmed heading and go towards it
rospy.loginfo('locking on and going')
rospy.loginfo('Headings: ' + str(userdata.headings))
finalHeading = userdata.headings[0]
if userdata.expectedLanes > 1:
# Assume we only have up to two lanes for now
angleDelta = userdata.headings[1] - userdata.headings[0]
leftIndex = 0 if 0 < angleDelta < 180 or angleDelta < -180 else 1
leftHeading, rightHeading = userdata.headings[
leftIndex], userdata.headings[1 - leftIndex]
if userdata.outDir == 'left':
finalHeading = leftHeading
elif userdata.outDir == 'right':
finalHeading = rightHeading
rospy.loginfo("Using heading %f" % finalHeading)
ctrl = controller()
ctrl.depth_setpoint = depth_setpoint
ctrl.heading_setpoint = finalHeading
if not TEST_MODE:
mission_srv(search_request=False,
task_complete_request=True,
task_complete_ctrl=ctrl)
else:
return 'killed'
return 'succeeded'
def searchComplete(self):
if not self.isAlone:
rospy.loginfo("Sending Complete request to mission planner")
self.toMission(fail_request=False,
task_complete_request=False,
search_request=True,
task_complete_ctrl=controller())
def searchComplete(self):
rospy.loginfo("Sending centered request to mission planner")
if not self.isAlone:
self.toMission(fail_request=False,
task_complete_request=False,
centered=True,
task_complete_ctrl=controller(
heading_setpoint=self.curHeading))
def taskComplete(self, heading=0.0):
rospy.loginfo("Sending Complete request to mission planner")
if not self.isAlone:
self.toMission(fail_request=False, task_complete_request=True,
task_complete_ctrl=controller(
heading_setpoint=heading))
self.canPublish = False
self.isAborted = True
self.motionClient.cancel_all_goals()
def execute(self, userdata):
if isAborted: return 'aborted'
ctrl = controller()
ctrl.depth_setpoint = depth_setpoint
ctrl.heading_setpoint = input_heading if not TEST_MODE else cur_heading
if not TEST_MODE:
mission_srv(search_request=False, task_complete_request=True, task_complete_ctrl=ctrl)
return 'succeeded'
def taskComplete(self, heading=0.0):
rospy.loginfo("Sending Complete request to mission planner")
if not self.isAlone:
self.toMission(
fail_request=False,
task_complete_request=True,
task_complete_ctrl=controller(heading_setpoint=heading))
self.canPublish = False
self.isAborted = True
self.motionClient.cancel_all_goals()
def handle_srv(self, req):
rospy.loginfo("RGB Service handled")
if req.start_request:
rospy.loginfo("RGB starting")
self.isStart = True
self.isAborted = False
self.canPublish = True
self.missionStart = time.time()
self.defaultDepth = req.start_ctrl.depth_setpoint
self.inputHeading = req.start_ctrl.heading_setpoint
self.curHeading = self.inputHeading
self.depthFromMission = self.defaultDepth
self.registerMission()
rospy.loginfo("Received heading: {}".format(self.inputHeading))
rospy.loginfo("Received depth: {}".format(self.defaultDepth))
return mission_to_visionResponse(start_response=True,
abort_response=False,
data=controller(heading_setpoint=self.inputHeading))
if req.abort_request:
rospy.loginfo("RGB abort received")
self.isAborted=True
# self.isStart = False
self.canPublish = False
self.sendMovement(forward=0.0, sidemove=0.0)
self.unregisterMission()
rospy.loginfo("Time taken: {}".format(time.time()-self.missionStart))
rospy.loginfo("Aborted complete")
return mission_to_visionResponse(start_response=False,
abort_response=True,
data=controller(heading_setpoint=self.curHeading))
def handleSrv(self, req):
if req.start_request:
rospy.loginfo("Received Start Request")
self.isAborted = False
self.defaultDepth = req.start_ctrl.depth_setpoint
self.inputHeading = req.start_ctrl.heading_setpoint
return mission_to_visionResponse(start_response=True,
abort_response=False,
data=controller(heading_setpoint=
self.curHeading))
elif req.abort_request:
rospy.loginfo("Received Abort Request!!!")
self.motionClient.cancel_all_goals()
self.isAborted = True
while not self.abortedDone:
rospy.sleep(rospy.Duration(0.3))
self.motionClient.cancel_all_goals()
return mission_to_visionResponse(start_response=False,
abort_response=True,
data=controller(heading_setpoint=
self.curHeading))
def execute(self, userdata):
if isAborted: return 'aborted'
ctrl = controller()
ctrl.depth_setpoint = depth_setpoint
ctrl.heading_setpoint = input_heading if not TEST_MODE else cur_heading
if not TEST_MODE:
mission_srv(search_request=False,
task_complete_request=True,
task_complete_ctrl=ctrl)
return 'succeeded'
def handle_srv(self, req):
global isStart
global isAborted
global locomotionGoal
rospy.loginfo("Pegs Service handled")
if req.start_request:
rospy.loginfo("Pegs starting")
self.isStart = True
self.isAborted = False
self.canPublish = True
self.defaultDepth = req.start_ctrl.depth_setpoint
self.inputHeading = req.start_ctrl.heading_setpoint
self.curHeading = self.inputHeading
self.registerMission()
rospy.loginfo("Received heading: {}".format(self.inputHeading))
rospy.loginfo("Received depth: {}".format(self.defaultDepth))
return mission_to_visionResponse(
start_response=True,
abort_response=False,
data=controller(heading_setpoint=self.curHeading))
elif req.abort_request:
rospy.loginfo("Pegs abort received")
self.sendMovement(forward=0.0, sidemove=0.0)
self.isAborted = True
self.isStart = False
self.canPublsih = False
self.unregisterMission()
return mission_to_visionResponse(
start_response=False,
abort_response=True,
data=controller(heading_setpoint=self.curHeading))
def handle_srv(self, req):
global isStart
global isAborted
global locomotionGoal
rospy.loginfo("Pegs Service handled")
if req.start_request:
rospy.loginfo("Pegs starting")
self.isStart = True
self.isAborted = False
self.canPublish = True
self.defaultDepth = req.start_ctrl.depth_setpoint
self.inputHeading = req.start_ctrl.heading_setpoint
self.curHeading = self.inputHeading
self.registerMission()
rospy.loginfo("Received heading: {}".format(self.inputHeading))
rospy.loginfo("Received depth: {}".format(self.defaultDepth))
return mission_to_visionResponse(start_response=True,
abort_response=False,
data=controller(heading_setpoint=
self.curHeading))
elif req.abort_request:
rospy.loginfo("Pegs abort received")
self.sendMovement(forward=0.0, sidemove=0.0)
self.isAborted=True
self.isStart = False
self.canPublsih = False
self.unregisterMission()
return mission_to_visionResponse(start_response=False,
abort_response=True,
data=controller(heading_setpoint=self.curHeading))
def abortMission(self):
rospy.loginfo("Aborted :( Sorry Mission Planner...")
if not self.isAlone:
self.toMission(fail_request=True, task_complete_request=False,
task_complete_ctrl=controller(
heading_setpoint=self.curHeading))
self.unregisterMission()
else:
self.unregister()
self.canPublish = False
self.isAborted = True
self.isKilled = True
rospy.loginfo("Aborted myself")
rospy.signal_shutdown("Bye")
def taskComplete(self):
rospy.loginfo("Yay! Task Complete!")
if not self.isAlone:
self.toMission(fail_request=False, task_complete_request=True,
task_complete_ctrl=controller(
heading_setpoint=self.curHeading))
rospy.loginfo("Sent to mission")
self.unregisterMission()
else:
self.unregister()
self.canPublish = False
self.isAborted = True
self.isKilled = True
# self.sendMovement(forward=0.0, sidemove=0.0)
rospy.signal_shutdown("Bye")
def abortMission(self):
rospy.loginfo("Aborted :( Sorry Mission Planner...")
if not self.isAlone:
self.toMission(fail_request=True,
task_complete_request=False,
task_complete_ctrl=controller(
heading_setpoint=self.curHeading))
self.unregisterMission()
else:
self.unregister()
self.canPublish = False
self.isAborted = True
self.isKilled = True
rospy.loginfo("Aborted myself")
rospy.signal_shutdown("Bye")
def taskComplete(self):
rospy.loginfo("Yay! Task Complete!")
if not self.isAlone:
self.toMission(fail_request=False,
task_complete_request=True,
task_complete_ctrl=controller(
heading_setpoint=self.curHeading))
rospy.loginfo("Sent to mission")
self.unregisterMission()
else:
self.unregister()
self.canPublish = False
self.isAborted = True
self.isKilled = True
# self.sendMovement(forward=0.0, sidemove=0.0)
rospy.signal_shutdown("Bye")
def execute(self, userdata):
rospy.loginfo("Waiting for buoy service")
self.world.RGBClient = rospy.ServiceProxy("/rgb/mission_to_vision",
mission_to_vision)
self.world.RGBClient.wait_for_service()
self.world.RGBClient(
start_request=True,
abort_request=False,
start_ctrl=controller(
depth_setpoint=2.5,
heading_setpoint=self.world.laneStatus['heading']))
while not rospy.is_shutdown():
if self.world.RGBStatus['done']:
return 'pass'
if self.world.laneStatus['aborted']:
return 'fail'
if self.world.laneStatus['failed']:
return 'fail'
def execute(self, userdata):
if isAborted: return 'aborted'
# Backoff a little
goal = bbauv_msgs.msg.ControllerGoal(
heading_setpoint = cur_heading,
depth_setpoint = depth_setpoint,
forward_setpoint = -1
)
actionClient.send_goal(goal)
actionClient.wait_for_result()
ctrl = controller()
ctrl.depth_setpoint = depth_setpoint
ctrl.heading_setpoint = cur_heading
if not TEST_MODE:
mission_srv(search_request=False, task_complete_request=True, task_complete_ctrl=ctrl)
return 'succeeded'
def execute(self, userdata):
rospy.loginfo("Waiting for buoy service")
self.world.RGBClient = rospy.ServiceProxy("/rgb/mission_to_vision",
mission_to_vision)
self.world.RGBClient.wait_for_service()
self.world.RGBClient(start_request=True,
abort_request=False,
start_ctrl=controller(
depth_setpoint=2.5,
heading_setpoint=
self.world.laneStatus['heading']
))
while not rospy.is_shutdown():
if self.world.RGBStatus['done']:
return 'pass'
if self.world.laneStatus['aborted']:
return 'fail'
if self.world.laneStatus['failed']:
return 'fail'
def execute(self, userdata):
self.world.laneStatus['numMarkers'] += 1;
if self.world.laneStatus['numMarkers'] > 5:
self.world.laneStatus['numMarkers'] = 1;
self.world.laneClient = rospy.ServiceProxy("/lane/mission_to_vision",
mission_to_vision)
if self.world.laneStatus['heading'] is None:
self.world.laneStatus['heading'] = self.world.current_yaw
rospy.loginfo("Waiting for lane server");
self.world.laneClient.wait_for_service()
rospy.loginfo("Done waiting for laen server")
self.world.enable_PID()
if(self.world.laneStatus['numMarkers'] == 5):
nLanes = 2;
else:
nLanes = 1;
rospy.loginfo("Number of markers: %s", str(
nLanes
))
self.world.laneClient(start_request=True, abort_request=False,
start_ctrl=controller(
depth_setpoint=0.3,
heading_setpoint=
self.world.laneStatus['heading'],
)
, numLanes = nLanes, chosenLane = 1
)
self.world.laneStatus['active'] = True
self.world.laneStatus['complete'] = False
self.world.laneStatus['aborted'] = False
rospy.loginfo("status of lane: %s", str(
self.world.laneStatus['complete']
))
while not rospy.is_shutdown():
if self.world.laneStatus['complete']:
return 'pass'
if self.world.laneStatus['aborted']:
return 'fail'
def execute(self, userdata):
self.world.laneStatus['numMarkers'] += 1
if self.world.laneStatus['numMarkers'] > 5:
self.world.laneStatus['numMarkers'] = 1
self.world.laneClient = rospy.ServiceProxy("/lane/mission_to_vision",
mission_to_vision)
if self.world.laneStatus['heading'] is None:
self.world.laneStatus['heading'] = self.world.current_yaw
rospy.loginfo("Waiting for lane server")
self.world.laneClient.wait_for_service()
rospy.loginfo("Done waiting for laen server")
self.world.enable_PID()
if (self.world.laneStatus['numMarkers'] == 5):
nLanes = 2
else:
nLanes = 1
rospy.loginfo("Number of markers: %s", str(nLanes))
self.world.laneClient(
start_request=True,
abort_request=False,
start_ctrl=controller(
depth_setpoint=0.3,
heading_setpoint=self.world.laneStatus['heading'],
),
numLanes=nLanes,
chosenLane=1)
self.world.laneStatus['active'] = True
self.world.laneStatus['complete'] = False
self.world.laneStatus['aborted'] = False
rospy.loginfo("status of lane: %s",
str(self.world.laneStatus['complete']))
while not rospy.is_shutdown():
if self.world.laneStatus['complete']:
return 'pass'
if self.world.laneStatus['aborted']:
return 'fail'
def main():
position = { 'x': 0, 'y': 0}
orientation = {'yaw': 0}
linear_vel = { 'x': 0, 'y': 0}
angular_vel = {'z': 0 }
def callback_cmd_vel(msg):
linear_vel['x'] = msg.linear.x #* -1
linear_vel['y'] = msg.linear.y #* -1
angular_vel['z'] = msg.angular.z
def callback_WHDVL(msg):
position['x'] = msg.pose.pose.position.x
position['y'] = msg.pose.pose.position.y
def callback_AHRS8(msg):
orientation['yaw'] = msg.orientation.z *(180/pi)
def integral(x_dot, x, t):
""" Computes the integral o x dt """
return (x_dot * t) + x
def normalize_angle(angle):
# Inspiration: http://www.ros.org/doc/api/angles/html/angles_8h_source.html; Normalizes the angle to be 0 to 360 It takes and returns degrees.
normalized = (angle%360+360)%360
return normalized
def forward_velocity(mb_fwd_vel, curr_Pos): #Forward Gains: Kp=5000, Kd=1500
lead_distance = 0
if abs(mb_fwd_vel) == 0:
lead_distance = 0
if abs(mb_fwd_vel) <=0.1: #0.25
lead_distance = 0.25
if abs(mb_fwd_vel) <=0.2: #0.5
lead_distance = 0.5
if abs(mb_fwd_vel) <=0.3:
lead_distance = 1 #1.75
if mb_fwd_vel > 0:
forward_setpoint = lead_distance + curr_Pos
if mb_fwd_vel < 0:
forward_setpoint = -1 * lead_distance + curr_Pos
return forward_setpoint
def sidemove_velocity(mb_side_vel, curr_Pos):
sidemove_setpoint = 0
lead_distance = 0
if abs(mb_side_vel) == 0:
lead_distance = 0
if abs(mb_side_vel) <=0.1:
lead_distance = 0.5
if abs(mb_side_vel) <=0.2:
lead_distance = 1
if mb_side_vel > 0:
sidemove_setpoint = lead_distance + curr_Pos
if mb_side_vel < 0:
sidemove_setpoint = -1 * lead_distance + curr_Pos
return sidemove_setpoint
def yaw_velocity(mb_yaw_vel, curr_Yaw): #Heading Gains: Kp = 40, Kd=25
lead_angle = 0
if abs(mb_yaw_vel) == 0:
lead_angle = 0
if abs(mb_yaw_vel) <=0.15: #5
lead_angle = 5
if abs(mb_yaw_vel) <=0.2: #7
lead_angle = 7
if abs(mb_yaw_vel) <=0.3: #10
lead_angle = 10
if abs(mb_yaw_vel) <=0.4: #15
lead_angle = 15
if abs(mb_yaw_vel) <=2: #20
lead_angle = 40
# if abs(mb_yaw_vel) <=1: #
# lead_angle = 30
if mb_yaw_vel > 0:
yaw_setpoint = normalize_angle(-1 * lead_angle + curr_Yaw)
if mb_yaw_vel < 0:
yaw_setpoint = normalize_angle(lead_angle + curr_Yaw)
return yaw_setpoint
#initialize node so roscore know who I am
rospy.init_node('cmd_position', anonymous=False)
#declare subscribing from what
cmd_vel_sub = rospy.Subscriber("/cmd_vel", Twist, callback_cmd_vel)
WH_DVL = rospy.Subscriber("/WH_DVL_data", Odometry, callback_WHDVL)
AHRS8 = rospy.Subscriber("/AHRS8_data_e", imu_data, callback_AHRS8)
#declare publishing to what
cmd_position = rospy.Publisher("/cmd_position", controller)
old_time = rospy.Time.now()
current_y = position['y']
while not rospy.is_shutdown():
# #code to check for period of each loop in secs
# t1 = rospy.Time.now()
# elapse = (t1-old_time).to_sec()
# #print elapse
# old_time = t1
output = controller()
#lead_distance = 1
lead_angle = -15 #degrees
lead_angle_onspot = -10
if linear_vel['x'] == None and angular_vel['z']== None:
rospy.loginfo("No mBase Cmds")
if linear_vel['x']==0 and angular_vel['z']==0:
current_x = position['x']
# current_y = position['y']
current_yaw = orientation['yaw']
while linear_vel['x']==0 and angular_vel['z']==0 and not rospy.is_shutdown():
#code to check for period of each loop in secs
t1 = rospy.Time.now()
elapse = (t1-old_time).to_sec()
#print elapse
old_time = t1
output.forward_setpoint = current_x
output.sidemove_setpoint = current_y
output.heading_setpoint = current_yaw
cmd_position.publish(output)
t2 = rospy.Time.now()
#print (t2-t1).to_sec()
p = 0.05 - (t2-t1).to_sec()
if p < 0.0 : p = 0.0
rospy.sleep(p)
rospy.loginfo("Exiting Stationary")
current_y = position['y']
if linear_vel['x']!=0 and angular_vel['z']!=0 :
#current_y = position['y']
while linear_vel['x']!=0 and angular_vel['z']!=0 and not rospy.is_shutdown():
#code to check for period of each loop in secs
t1 = rospy.Time.now()
elapse = (t1-old_time).to_sec()
#print elapse
old_time = t1
output.forward_setpoint = forward_velocity(linear_vel['x'], position['x'])
output.sidemove_setpoint = current_y
# output.heading_setpoint = current_yaw
output.heading_setpoint = yaw_velocity(angular_vel['z'], orientation['yaw'])
# if angular_vel['z']>0:
# output.heading_setpoint = normalize_angle(lead_angle_onspot + orientation['yaw']) #normalize_angle(integral(converted_angular_vel['z'], orientation['yaw'], 0.05))
# if angular_vel['z']<0:
# output.heading_setpoint = normalize_angle(-1 * lead_angle_onspot + orientation['yaw']) #normalize_angle(integral(converted_angular_vel['z'], orientation['yaw'], 0.05))
# rospy.loginfo("Error on AngZ= %2.5f", output.heading_setpoint-orientation['yaw'])
cmd_position.publish(output)
t2 = rospy.Time.now()
#print (t2-t1).to_sec()
p = 0.05 - (t2-t1).to_sec()
if p < 0.0 : p = 0.0
rospy.sleep(p)
#output.forward_setpoint = position['x'] + 0.45
#cmd_position.publish(output)
if linear_vel['x']==0 and angular_vel['z']!=0:
#current_x = position['x']
#current_y = position['y']
while linear_vel['x']==0 and angular_vel['z']!=0 and not rospy.is_shutdown():
#code to check for period of each loop in secs
t1 = rospy.Time.now()
elapse = (t1-old_time).to_sec()
#print elapse
old_time = t1
output.forward_setpoint = current_x
output.sidemove_setpoint = current_y
output.heading_setpoint = yaw_velocity(angular_vel['z'], orientation['yaw'])
# if angular_vel['z']>0:
# output.heading_setpoint = normalize_angle(lead_angle + orientation['yaw']) #normalize_angle(integral(converted_angular_vel['z'], orientation['yaw'], 0.05))
# if angular_vel['z']<0:
# output.heading_setpoint = normalize_angle(-1 * lead_angle + orientation['yaw']) #normalize_angle(integral(converted_angular_vel['z'], orientation['yaw'], 0.05))
# rospy.loginfo("Error on AngZ= %2.5f", output.heading_setpoint-orientation['yaw'])
cmd_position.publish(output)
t2 = rospy.Time.now()
#print (t2-t1).to_sec()
p = 0.05 - (t2-t1).to_sec()
if p < 0.0 : p = 0.0
rospy.sleep(p)
if linear_vel['x']!=0 and angular_vel['z']==0:
#current_y = position['y']
current_yaw = orientation['yaw']
while linear_vel['x']!=0 and angular_vel['z']==0 and not rospy.is_shutdown():
#code to check for period of each loop in secs
t1 = rospy.Time.now()
elapse = (t1-old_time).to_sec()
#print elapse
old_time = t1
output.forward_setpoint = forward_velocity(linear_vel['x'], position['x'])
# if linear_vel['x']>0:
# output.forward_setpoint = lead_distance + position['x'] # + integral(linear_vel['x'], position['x'], 0.05) + lead_distance
# if linear_vel['x']<0:
# output.forward_setpoint = -1 * lead_distance + position['x'] # + integral(linear_vel['x'], position['x'], 0.05) + lead_distance
rospy.loginfo("Error on X= %2.5f", output.forward_setpoint-position['x'])
output.sidemove_setpoint = current_y
output.heading_setpoint = current_yaw
cmd_position.publish(output)
t2 = rospy.Time.now()
#print (t2-t1).to_sec()
p = 0.05 - (t2-t1).to_sec()
if p < 0.0 : p = 0.0
rospy.sleep(p)
#output.forward_setpoint = position['x'] + 0.45
#cmd_position.publish(output)
# t2 = rospy.Time.now()
# #print (t2-t1).to_sec()
# p = 0.05 - (t2-t1).to_sec()
# if p < 0.0 : p = 0.0
rospy.sleep(0)
def main():
position = {'x': 0, 'y': 0}
orientation = {'yaw': 0}
linear_vel = {'x': 0, 'y': 0}
angular_vel = {'z': 0}
def callback_cmd_vel(msg):
linear_vel['x'] = msg.linear.x #* -1
linear_vel['y'] = msg.linear.y #* -1
angular_vel['z'] = msg.angular.z
def callback_WHDVL(msg):
position['x'] = msg.pose.pose.position.x
position['y'] = msg.pose.pose.position.y
def callback_AHRS8(msg):
orientation['yaw'] = msg.orientation.z * (180 / pi)
def integral(x_dot, x, t):
""" Computes the integral o x dt """
return (x_dot * t) + x
def normalize_angle(angle):
# Inspiration: http://www.ros.org/doc/api/angles/html/angles_8h_source.html; Normalizes the angle to be 0 to 360 It takes and returns degrees.
normalized = (angle % 360 + 360) % 360
return normalized
def forward_velocity(mb_fwd_vel,
curr_Pos): #Forward Gains: Kp=5000, Kd=1500
lead_distance = 0
if abs(mb_fwd_vel) == 0:
lead_distance = 0
if abs(mb_fwd_vel) <= 0.1: #0.25
lead_distance = 0.25
if abs(mb_fwd_vel) <= 0.2: #0.5
lead_distance = 0.5
if abs(mb_fwd_vel) <= 0.3:
lead_distance = 1 #1.75
if mb_fwd_vel > 0:
forward_setpoint = lead_distance + curr_Pos
if mb_fwd_vel < 0:
forward_setpoint = -1 * lead_distance + curr_Pos
return forward_setpoint
def sidemove_velocity(mb_side_vel, curr_Pos):
sidemove_setpoint = 0
lead_distance = 0
if abs(mb_side_vel) == 0:
lead_distance = 0
if abs(mb_side_vel) <= 0.1:
lead_distance = 0.5
if abs(mb_side_vel) <= 0.2:
lead_distance = 1
if mb_side_vel > 0:
sidemove_setpoint = lead_distance + curr_Pos
if mb_side_vel < 0:
sidemove_setpoint = -1 * lead_distance + curr_Pos
return sidemove_setpoint
def yaw_velocity(mb_yaw_vel, curr_Yaw): #Heading Gains: Kp = 40, Kd=25
lead_angle = 0
if abs(mb_yaw_vel) == 0:
lead_angle = 0
if abs(mb_yaw_vel) <= 0.15: #5
lead_angle = 5
if abs(mb_yaw_vel) <= 0.2: #7
lead_angle = 7
if abs(mb_yaw_vel) <= 0.3: #10
lead_angle = 10
if abs(mb_yaw_vel) <= 0.4: #15
lead_angle = 15
if abs(mb_yaw_vel) <= 2: #20
lead_angle = 40
# if abs(mb_yaw_vel) <=1: #
# lead_angle = 30
if mb_yaw_vel > 0:
yaw_setpoint = normalize_angle(-1 * lead_angle + curr_Yaw)
if mb_yaw_vel < 0:
yaw_setpoint = normalize_angle(lead_angle + curr_Yaw)
return yaw_setpoint
#initialize node so roscore know who I am
rospy.init_node('cmd_position', anonymous=False)
#declare subscribing from what
cmd_vel_sub = rospy.Subscriber("/cmd_vel", Twist, callback_cmd_vel)
WH_DVL = rospy.Subscriber("/WH_DVL_data", Odometry, callback_WHDVL)
AHRS8 = rospy.Subscriber("/AHRS8_data_e", imu_data, callback_AHRS8)
#declare publishing to what
cmd_position = rospy.Publisher("/cmd_position", controller)
old_time = rospy.Time.now()
current_y = position['y']
while not rospy.is_shutdown():
# #code to check for period of each loop in secs
# t1 = rospy.Time.now()
# elapse = (t1-old_time).to_sec()
# #print elapse
# old_time = t1
output = controller()
#lead_distance = 1
lead_angle = -15 #degrees
lead_angle_onspot = -10
if linear_vel['x'] == None and angular_vel['z'] == None:
rospy.loginfo("No mBase Cmds")
if linear_vel['x'] == 0 and angular_vel['z'] == 0:
current_x = position['x']
# current_y = position['y']
current_yaw = orientation['yaw']
while linear_vel['x'] == 0 and angular_vel[
'z'] == 0 and not rospy.is_shutdown():
#code to check for period of each loop in secs
t1 = rospy.Time.now()
elapse = (t1 - old_time).to_sec()
#print elapse
old_time = t1
output.forward_setpoint = current_x
output.sidemove_setpoint = current_y
output.heading_setpoint = current_yaw
cmd_position.publish(output)
t2 = rospy.Time.now()
#print (t2-t1).to_sec()
p = 0.05 - (t2 - t1).to_sec()
if p < 0.0: p = 0.0
rospy.sleep(p)
rospy.loginfo("Exiting Stationary")
current_y = position['y']
if linear_vel['x'] != 0 and angular_vel['z'] != 0:
#current_y = position['y']
while linear_vel['x'] != 0 and angular_vel[
'z'] != 0 and not rospy.is_shutdown():
#code to check for period of each loop in secs
t1 = rospy.Time.now()
elapse = (t1 - old_time).to_sec()
#print elapse
old_time = t1
output.forward_setpoint = forward_velocity(
linear_vel['x'], position['x'])
output.sidemove_setpoint = current_y
# output.heading_setpoint = current_yaw
output.heading_setpoint = yaw_velocity(angular_vel['z'],
orientation['yaw'])
# if angular_vel['z']>0:
# output.heading_setpoint = normalize_angle(lead_angle_onspot + orientation['yaw']) #normalize_angle(integral(converted_angular_vel['z'], orientation['yaw'], 0.05))
# if angular_vel['z']<0:
# output.heading_setpoint = normalize_angle(-1 * lead_angle_onspot + orientation['yaw']) #normalize_angle(integral(converted_angular_vel['z'], orientation['yaw'], 0.05))
# rospy.loginfo("Error on AngZ= %2.5f", output.heading_setpoint-orientation['yaw'])
cmd_position.publish(output)
t2 = rospy.Time.now()
#print (t2-t1).to_sec()
p = 0.05 - (t2 - t1).to_sec()
if p < 0.0: p = 0.0
rospy.sleep(p)
#output.forward_setpoint = position['x'] + 0.45
#cmd_position.publish(output)
if linear_vel['x'] == 0 and angular_vel['z'] != 0:
#current_x = position['x']
#current_y = position['y']
while linear_vel['x'] == 0 and angular_vel[
'z'] != 0 and not rospy.is_shutdown():
#code to check for period of each loop in secs
t1 = rospy.Time.now()
elapse = (t1 - old_time).to_sec()
#print elapse
old_time = t1
output.forward_setpoint = current_x
output.sidemove_setpoint = current_y
output.heading_setpoint = yaw_velocity(angular_vel['z'],
orientation['yaw'])
# if angular_vel['z']>0:
# output.heading_setpoint = normalize_angle(lead_angle + orientation['yaw']) #normalize_angle(integral(converted_angular_vel['z'], orientation['yaw'], 0.05))
# if angular_vel['z']<0:
# output.heading_setpoint = normalize_angle(-1 * lead_angle + orientation['yaw']) #normalize_angle(integral(converted_angular_vel['z'], orientation['yaw'], 0.05))
# rospy.loginfo("Error on AngZ= %2.5f", output.heading_setpoint-orientation['yaw'])
cmd_position.publish(output)
t2 = rospy.Time.now()
#print (t2-t1).to_sec()
p = 0.05 - (t2 - t1).to_sec()
if p < 0.0: p = 0.0
rospy.sleep(p)
if linear_vel['x'] != 0 and angular_vel['z'] == 0:
#current_y = position['y']
current_yaw = orientation['yaw']
while linear_vel['x'] != 0 and angular_vel[
'z'] == 0 and not rospy.is_shutdown():
#code to check for period of each loop in secs
t1 = rospy.Time.now()
elapse = (t1 - old_time).to_sec()
#print elapse
old_time = t1
output.forward_setpoint = forward_velocity(
linear_vel['x'], position['x'])
# if linear_vel['x']>0:
# output.forward_setpoint = lead_distance + position['x'] # + integral(linear_vel['x'], position['x'], 0.05) + lead_distance
# if linear_vel['x']<0:
# output.forward_setpoint = -1 * lead_distance + position['x'] # + integral(linear_vel['x'], position['x'], 0.05) + lead_distance
rospy.loginfo("Error on X= %2.5f",
output.forward_setpoint - position['x'])
output.sidemove_setpoint = current_y
output.heading_setpoint = current_yaw
cmd_position.publish(output)
t2 = rospy.Time.now()
#print (t2-t1).to_sec()
p = 0.05 - (t2 - t1).to_sec()
if p < 0.0: p = 0.0
rospy.sleep(p)
#output.forward_setpoint = position['x'] + 0.45
#cmd_position.publish(output)
# t2 = rospy.Time.now()
# #print (t2-t1).to_sec()
# p = 0.05 - (t2-t1).to_sec()
# if p < 0.0 : p = 0.0
rospy.sleep(0)
def handle_srv(self, req):
global isStart
global isAborted
global locomotionGoal
rospy.loginfo("Torpedo Service handled")
if req.start_request:
rospy.loginfo("Torpedo starting")
self.isStart = True
self.isAborted = False
self.canPublish = True
self.defaultDepth = req.start_ctrl.depth_setpoint
self.inputHeading = req.start_ctrl.heading_setpoint
self.curHeading = self.inputHeading
self.depthFromMission = self.defaultDepth
rospy.loginfo("Received depth: {}".format(self.defaultDepth))
rospy.loginfo("Received heading: {}".format(self.inputHeading))
self.registerMission()
self.regCompass()
# self.registerSonar()
# self.curHeading = self.curHeading - 10.0
# self.sendMovement(forward=0.0, sidemove=0.0,
# heading=self.curHeading, blocking=True)
return mission_to_visionResponse(start_response=True,
abort_response=False,
data=controller(heading_setpoint=
self.curHeading))
elif req.abort_request:
rospy.loginfo("Torpedo abort received")
self.isAborted=True
# self.isStart = False
self.canPublish = False
self.unregisterMission()
# Randomly shoot stuff
if self.numShoot == 0:
self.shootTopTorpedo()
else:
self.shootBotTorpedo()
self.sendMovement(forward=0.0, sidemove=0.0)
rospy.loginfo("Aborted complete")
rospy.loginfo("Time taken: {}".format(time.time()-self.missionStart))
# if self.isMovingState:
# return mission_to_visionResponse(start_response=False,
# abort_response=True,
# search_request=True,
# data=controller(heading_setpoint=
# self.curHeading))
# else:
return mission_to_visionResponse(start_response=False,
abort_response=True,
data=controller(heading_setpoint=
self.curHeading))
def searchComplete(self):
if not self.isAlone:
rospy.loginfo("Sending Complete request to mission planner")
self.toMission(fail_request=False, task_complete_request=False,
search_request=True,
task_complete_ctrl=controller())