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 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 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 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))