def send_position_goal(stateRefPub, position): stateRef = NavSts() stateRef.header.seq = 0 stateRef.header.stamp.secs = 0 stateRef.header.stamp.nsecs = 0 stateRef.global_position.latitude = 0.0 stateRef.global_position.longitude = 0.0 stateRef.origin.latitude = 0.0 stateRef.origin.longitude = 0.0 stateRef.position.north = position.north stateRef.position.east = position.east stateRef.position.depth = position.depth stateRef.altitude = 0.0 stateRef.body_velocity.x = 0 stateRef.body_velocity.y = 0 stateRef.body_velocity.z = 0 stateRef.gbody_velocity.x = 0 stateRef.gbody_velocity.y = 0 stateRef.gbody_velocity.z = 0 stateRef.orientation.roll = 0 stateRef.orientation.pitch = 0 stateRef.orientation.yaw = 0 stateRef.orientation_rate.roll = 0 stateRef.orientation_rate.pitch = 0 stateRef.orientation_rate.yaw = 0 stateRef.position_variance.north = 0 stateRef.position_variance.east = 0 stateRef.position_variance.depth = 0 stateRef.orientation_variance.roll = 0 stateRef.orientation_variance.pitch = 0 stateRef.orientation_variance.yaw = 0 stateRef.status = 0 send_state_ref(stateRefPub, stateRef)
def send_position_goal(stateRefPub, position): stateRef = NavSts() stateRef.header.seq = 0 stateRef.header.stamp.secs = 0 stateRef.header.stamp.nsecs = 0 stateRef.global_position.latitude = 0.0 stateRef.global_position.longitude = 0.0 stateRef.origin.latitude = 0.0 stateRef.origin.longitude = 0.0 stateRef.position.north = position.north stateRef.position.east = position.east stateRef.position.depth = position.depth stateRef.altitude = 0.0 stateRef.body_velocity.x = 0 stateRef.body_velocity.y = 0 stateRef.body_velocity.z = 0 stateRef.gbody_velocity.x = 0 stateRef.gbody_velocity.y = 0 stateRef.gbody_velocity.z = 0 stateRef.orientation.roll = 0 stateRef.orientation.pitch = 0 stateRef.orientation.yaw = 0 stateRef.orientation_rate.roll = 0 stateRef.orientation_rate.pitch = 0 stateRef.orientation_rate.yaw = 0 stateRef.position_variance.north = 0 stateRef.position_variance.east = 0 stateRef.position_variance.depth = 0 stateRef.orientation_variance.roll = 0 stateRef.orientation_variance.pitch = 0 stateRef.orientation_variance.yaw = 0 stateRef.status = 0 send_state_ref(stateRefPub, stateRef)
def odometry_cb(self, odom): quaternion = (odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) msg = NavSts() msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'map' llh = self.ned.ned2geodetic( [odom.pose.pose.position.x, odom.pose.pose.position.y, 0]) msg.global_position.latitude = llh[0] msg.global_position.longitude = llh[1] msg.position.north = odom.pose.pose.position.x msg.position.east = odom.pose.pose.position.y msg.position.depth = 0.0 msg.altitude = 0.0 msg.body_velocity.x = odom.twist.twist.linear.x msg.body_velocity.y = odom.twist.twist.linear.y msg.body_velocity.z = odom.twist.twist.linear.z msg.orientation.roll = euler[0] msg.orientation.pitch = euler[1] msg.orientation.yaw = euler[2] msg.origin.latitude = self.origin_latitude msg.origin.longitude = self.origin_longitude self.xiroi_pose_pub.publish(msg) msg.header.seq = msg.header.seq + 1
def send_depth_goal(stateRefPub, position): try: depth_enable = rospy.ServiceProxy('DEPTH_enable', EnableControl) depth_enable(enable=True) velcon_enable = rospy.ServiceProxy('VelCon_enable', EnableControl) velcon_enable(enable=True) config_vel_controller = rospy.ServiceProxy( 'ConfigureVelocityController', ConfigureVelocityController) config_vel_controller(ControllerName="DEPTH", desired_mode=[0, 0, 2, 0, 0, 0]) stateRef = NavSts() stateRef.header.seq = 0 stateRef.header.stamp.secs = 0 stateRef.header.stamp.nsecs = 0 stateRef.global_position.latitude = 0.0 stateRef.global_position.longitude = 0.0 stateRef.origin.latitude = 0.0 stateRef.origin.longitude = 0.0 stateRef.position.north = position.north stateRef.position.east = position.east stateRef.position.depth = position.depth stateRef.altitude = 0.0 stateRef.body_velocity.x = 0 stateRef.body_velocity.y = 0 stateRef.body_velocity.z = 0 stateRef.gbody_velocity.x = 0 stateRef.gbody_velocity.y = 0 stateRef.gbody_velocity.z = 0 stateRef.orientation.roll = 0 stateRef.orientation.pitch = 0 stateRef.orientation.yaw = 0 stateRef.orientation_rate.roll = 0 stateRef.orientation_rate.pitch = 0 stateRef.orientation_rate.yaw = 0 stateRef.position_variance.north = 0 stateRef.position_variance.east = 0 stateRef.position_variance.depth = 0 stateRef.orientation_variance.roll = 0 stateRef.orientation_variance.pitch = 0 stateRef.orientation_variance.yaw = 0 stateRef.status = 0 stateRefPub.publish(stateRef) return 0 except rospy.ServiceException, e: rospy.logerr("Failed to call change depth action" % e) return -1
def send_depth_goal(stateRefPub, position): try: depth_enable = rospy.ServiceProxy('DEPTH_enable', EnableControl) depth_enable(enable=True) velcon_enable = rospy.ServiceProxy('VelCon_enable', EnableControl) velcon_enable(enable=True) config_vel_controller = rospy.ServiceProxy('ConfigureVelocityController', ConfigureVelocityController) config_vel_controller(ControllerName="DEPTH", desired_mode=[0, 0, 2, 0, 0, 0]) stateRef = NavSts() stateRef.header.seq = 0 stateRef.header.stamp.secs = 0 stateRef.header.stamp.nsecs = 0 stateRef.global_position.latitude = 0.0 stateRef.global_position.longitude = 0.0 stateRef.origin.latitude = 0.0 stateRef.origin.longitude = 0.0 stateRef.position.north = position.north stateRef.position.east = position.east stateRef.position.depth = position.depth stateRef.altitude = 0.0 stateRef.body_velocity.x = 0 stateRef.body_velocity.y = 0 stateRef.body_velocity.z = 0 stateRef.gbody_velocity.x = 0 stateRef.gbody_velocity.y = 0 stateRef.gbody_velocity.z = 0 stateRef.orientation.roll = 0 stateRef.orientation.pitch = 0 stateRef.orientation.yaw = 0 stateRef.orientation_rate.roll = 0 stateRef.orientation_rate.pitch = 0 stateRef.orientation_rate.yaw = 0 stateRef.position_variance.north = 0 stateRef.position_variance.east = 0 stateRef.position_variance.depth = 0 stateRef.orientation_variance.roll = 0 stateRef.orientation_variance.pitch = 0 stateRef.orientation_variance.yaw = 0 stateRef.status = 0 stateRefPub.publish(stateRef) return 0 except rospy.ServiceException, e: rospy.logerr("Failed to call change depth action" % e) return -1
def __init__(self): # position self.position = Point(0, 0, 0) rospy.Subscriber('position', NavSts, self.position_cb) # state reference publisher self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1) self.action_rec_flag = 0 # 0 - waiting action, 1 - received action, 2 - executing action self.as_goal = aPadGoal() self.as_res = aPadResult() self.as_feed = aPadFeedback() # TODO create 4 docking stations -- 4 offset positions for every station + before docking, align the object to station self.position_offset = Point(-0.5, -0.5, 0) # for docking -- object offset self.pos_old = Point(self.position.x, self.position.y, self.position.z) self.pos_err = [] self.perched_flag = 0 # flag for dummy action perching onto aMussel/aFish/other objects action self.perched_count = 0 # number of currently docked objects (default max 4) # initialize actions server structures self.action_server = actionlib.SimpleActionServer("action_server", aPadAction, auto_start=False) self.action_server.register_goal_callback(self.action_execute_cb) self.action_server.register_preempt_callback(self.action_preempt_cb) self.action_server.start() while not rospy.is_shutdown(): # goal position for aMussel/aFish/object docked onto aPad (if perched!) #TODO multiple docking stations goalPosition = NED() goalPosition.north = self.position.x + self.position_offset.x goalPosition.east = self.position.y + self.position_offset.y #goalPosition.z = self.position.z + self.position_offset.z if self.perched_flag == 1: self.set_model_state(goalPosition) if self.action_server.is_active(): ################### # received action # ################### if self.action_rec_flag == 1: if self.as_goal.id == 0: print 'Go to position action' # 2d movement self.pos_old = Point(self.position.x, self.position.y, 0) start = Vector3(self.position.x, self.position.y, 0) end = Vector3(self.as_goal.pose.position.x, self.as_goal.pose.position.y, 0) dl = sqrt( pow(self.as_goal.pose.position.x - self.position.x, 2) + pow(self.as_goal.pose.position.y - self.position.y, 2)) # if within 10cm of goal if dl < 0.1: self.action_rec_flag = 0 # waiting for new action self.as_res.status = 0 self.pos_err = [] print 'finished executing go_to_position action' print "###" print self.position print "###" self.action_server.set_succeeded(self.as_res) else: try: # send goal fadp_enable = rospy.ServiceProxy( 'FADP_enable', EnableControl) fadp_enable(enable=True) config_vel_controller = rospy.ServiceProxy( 'ConfigureVelocityController', ConfigureVelocityController) config_vel_controller( ControllerName="FADP", desired_mode=[2, 2, 0, 0, 0, 0]) velcon_enable = rospy.ServiceProxy( 'VelCon_enable', EnableControl) velcon_enable(enable=True) stateRef = NavSts() stateRef.header.seq = 0 stateRef.header.stamp.secs = 0 stateRef.header.stamp.nsecs = 0 stateRef.global_position.latitude = 0.0 stateRef.global_position.longitude = 0.0 stateRef.origin.latitude = 0.0 stateRef.origin.longitude = 0.0 stateRef.position.north = self.as_goal.pose.position.x stateRef.position.east = self.as_goal.pose.position.y stateRef.position.depth = 0 stateRef.altitude = 0.0 stateRef.body_velocity.x = 0 stateRef.body_velocity.y = 0 stateRef.body_velocity.z = 0 stateRef.gbody_velocity.x = 0 stateRef.gbody_velocity.y = 0 stateRef.gbody_velocity.z = 0 stateRef.orientation.roll = 0 stateRef.orientation.pitch = 0 stateRef.orientation.yaw = 0 stateRef.orientation_rate.roll = 0 stateRef.orientation_rate.pitch = 0 stateRef.orientation_rate.yaw = 0 stateRef.position_variance.north = 0 stateRef.position_variance.east = 0 stateRef.position_variance.depth = 0 stateRef.orientation_variance.roll = 0 stateRef.orientation_variance.pitch = 0 stateRef.orientation_variance.yaw = 0 stateRef.status = 0 self.stateRefPub.publish(stateRef) self.action_rec_flag = 2 #executing trajectory except rospy.ServiceException, e: print "Service for activating controller failed: %s" % e self.as_res.status = -1 self.action_server.set_aborted(self.as_res) self.action_rec_flag = 0 # waiting for new actions elif self.as_goal.id == 1: print 'perch action' self.action_rec_flag = 2 # start executing action elif self.as_goal.id == 2: print 'release action' self.action_rec_flag = 2 # start executing action #################### # executing action # #################### elif self.action_rec_flag == 2: self.as_res.id = self.as_goal.id self.as_feed.id = self.as_goal.id if self.as_goal.id == 0: # Go to position action dL = sqrt( pow(self.as_goal.pose.position.x - self.pos_old.x, 2) + pow(self.as_goal.pose.position.y - self.pos_old.y, 2)) dl = sqrt( pow(self.as_goal.pose.position.x - self.position.x, 2) + pow(self.as_goal.pose.position.y - self.position.y, 2)) if len(self.pos_err) < 20: self.pos_err.append(dl) else: self.pos_err.pop(0) self.pos_err.append(dl) if (len(self.pos_err) == 20) and ( fabs(sum(self.pos_err) / len(self.pos_err)) < 0.5): # mission is successfully finished self.action_rec_flag = 0 # waiting for new action self.as_res.status = 0 self.pos_err = [] print 'finished executing go_to_position action' self.action_server.set_succeeded(self.as_res) else: # mission is still ongoing self.as_feed.status = ( 1 - dl / dL) * 100 # mission completeness self.as_feed.pose.position = self.position #print [fabs(sum(self.pos_err) / len(self.pos_err)), str(self.position), str(self.as_goal.pose.position)] self.action_server.publish_feedback(self.as_feed) elif self.as_goal.id == 1: #if self.perched_count==4: #return print 'executing perch action' self.perched_flag = 1 # static position publisher #TODO make 4 static pose publishers, for 4 docking stations self.staticPosPub = rospy.Publisher( '/' + self.as_goal.object + '/position_static', NavSts, queue_size=1) print 'finished executing perch action ' + self.as_goal.object self.action_rec_flag = 0 # waiting for new action self.as_res.status = 0 #self.perched_count+=1 self.action_server.set_succeeded(self.as_res) elif self.as_goal.id == 2: #if self.perched_count==4: #return print 'executing release action' self.perched_flag = 0 msg = NavSts() msg.position = NED( self.position.x + self.position_offset.x, self.position.y + self.position_offset.y, 0) msg.status = 1 # status 1 -- the last static position, give control back to uvsim #TODO make 4 static pose publishers, for 4 docking stations self.staticPosPub.publish( msg ) # signal the end of static position to attached object print 'finished executing release action' self.action_rec_flag = 0 # waiting for new action self.as_res.status = 0 #self.perched_count-=1 self.action_server.set_succeeded(self.as_res) else: pass rospy.sleep(rospy.Duration(0.05))
def __init__(self): # position self.position = Point(0, 0, 0) rospy.Subscriber('position', NavSts, self.position_cb) # state reference publisher self.stateRefPub = rospy.Publisher('stateRef', NavSts, queue_size=1) self.action_rec_flag = 0 # 0 - waiting action, 1 - received action, 2 - executing action self.as_goal = aPadGoal() self.as_res = aPadResult() self.as_feed = aPadFeedback() # TODO create 4 docking stations -- 4 offset positions for every station + before docking, align the object to station self.position_offset = Point(-0.5, -0.5, 0) # for docking -- object offset self.pos_old = Point(self.position.x, self.position.y, self.position.z) self.pos_err = [] self.perched_flag = 0 # flag for dummy action perching onto aMussel/aFish/other objects action self.perched_count = 0 # number of currently docked objects (default max 4) # initialize actions server structures self.action_server = actionlib.SimpleActionServer("action_server", aPadAction, auto_start=False) self.action_server.register_goal_callback(self.action_execute_cb) self.action_server.register_preempt_callback(self.action_preempt_cb) self.action_server.start() while not rospy.is_shutdown(): # goal position for aMussel/aFish/object docked onto aPad (if perched!) #TODO multiple docking stations goalPosition = NED() goalPosition.north = self.position.x + self.position_offset.x goalPosition.east = self.position.y + self.position_offset.y #goalPosition.z = self.position.z + self.position_offset.z if self.perched_flag == 1: self.set_model_state(goalPosition) if self.action_server.is_active(): ################### # received action # ################### if self.action_rec_flag == 1: if self.as_goal.id == 0: print 'Go to position action' # 2d movement self.pos_old = Point(self.position.x, self.position.y, 0) start = Vector3(self.position.x, self.position.y, 0) end = Vector3(self.as_goal.pose.position.x, self.as_goal.pose.position.y, 0) dl = sqrt(pow(self.as_goal.pose.position.x - self.position.x, 2) + pow(self.as_goal.pose.position.y - self.position.y, 2)) # if within 10cm of goal if dl < 0.1: self.action_rec_flag = 0 # waiting for new action self.as_res.status = 0 self.pos_err = [] print 'finished executing go_to_position action' print "###" print self.position print "###" self.action_server.set_succeeded(self.as_res) else: try: # send goal fadp_enable = rospy.ServiceProxy('FADP_enable', EnableControl) fadp_enable(enable=True) config_vel_controller = rospy.ServiceProxy('ConfigureVelocityController', ConfigureVelocityController) config_vel_controller(ControllerName="FADP", desired_mode=[2, 2, 0, 0, 0, 0]) velcon_enable = rospy.ServiceProxy('VelCon_enable', EnableControl) velcon_enable(enable=True) stateRef = NavSts() stateRef.header.seq = 0 stateRef.header.stamp.secs = 0 stateRef.header.stamp.nsecs = 0 stateRef.global_position.latitude = 0.0 stateRef.global_position.longitude = 0.0 stateRef.origin.latitude = 0.0 stateRef.origin.longitude = 0.0 stateRef.position.north = self.as_goal.pose.position.x stateRef.position.east = self.as_goal.pose.position.y stateRef.position.depth = 0 stateRef.altitude = 0.0 stateRef.body_velocity.x = 0 stateRef.body_velocity.y = 0 stateRef.body_velocity.z = 0 stateRef.gbody_velocity.x = 0 stateRef.gbody_velocity.y = 0 stateRef.gbody_velocity.z = 0 stateRef.orientation.roll = 0 stateRef.orientation.pitch = 0 stateRef.orientation.yaw = 0 stateRef.orientation_rate.roll = 0 stateRef.orientation_rate.pitch = 0 stateRef.orientation_rate.yaw = 0 stateRef.position_variance.north = 0 stateRef.position_variance.east = 0 stateRef.position_variance.depth = 0 stateRef.orientation_variance.roll = 0 stateRef.orientation_variance.pitch = 0 stateRef.orientation_variance.yaw = 0 stateRef.status = 0 self.stateRefPub.publish(stateRef) self.action_rec_flag = 2 #executing trajectory except rospy.ServiceException, e: print "Service for activating controller failed: %s" % e self.as_res.status = -1 self.action_server.set_aborted(self.as_res) self.action_rec_flag = 0 # waiting for new actions elif self.as_goal.id == 1: print 'perch action' self.action_rec_flag = 2 # start executing action elif self.as_goal.id == 2: print 'release action' self.action_rec_flag = 2 # start executing action #################### # executing action # #################### elif self.action_rec_flag == 2: self.as_res.id = self.as_goal.id self.as_feed.id = self.as_goal.id if self.as_goal.id == 0: # Go to position action dL = sqrt(pow(self.as_goal.pose.position.x - self.pos_old.x, 2) + pow(self.as_goal.pose.position.y - self.pos_old.y, 2)) dl = sqrt(pow(self.as_goal.pose.position.x - self.position.x, 2) + pow(self.as_goal.pose.position.y - self.position.y, 2)) if len(self.pos_err) < 20: self.pos_err.append(dl) else: self.pos_err.pop(0) self.pos_err.append(dl) if (len(self.pos_err) == 20) and (fabs(sum(self.pos_err) / len(self.pos_err)) < 0.5): # mission is successfully finished self.action_rec_flag = 0 # waiting for new action self.as_res.status = 0 self.pos_err = [] print 'finished executing go_to_position action' self.action_server.set_succeeded(self.as_res) else: # mission is still ongoing self.as_feed.status = (1 - dl / dL) * 100 # mission completeness self.as_feed.pose.position = self.position #print [fabs(sum(self.pos_err) / len(self.pos_err)), str(self.position), str(self.as_goal.pose.position)] self.action_server.publish_feedback(self.as_feed) elif self.as_goal.id == 1: #if self.perched_count==4: #return print 'executing perch action' self.perched_flag = 1 # static position publisher #TODO make 4 static pose publishers, for 4 docking stations self.staticPosPub = rospy.Publisher('/' + self.as_goal.object + '/position_static', NavSts, queue_size=1) print 'finished executing perch action ' + self.as_goal.object self.action_rec_flag = 0 # waiting for new action self.as_res.status = 0 #self.perched_count+=1 self.action_server.set_succeeded(self.as_res) elif self.as_goal.id == 2: #if self.perched_count==4: #return print 'executing release action' self.perched_flag = 0 msg = NavSts() msg.position = NED(self.position.x + self.position_offset.x, self.position.y + self.position_offset.y, 0) msg.status = 1 # status 1 -- the last static position, give control back to uvsim #TODO make 4 static pose publishers, for 4 docking stations self.staticPosPub.publish(msg) # signal the end of static position to attached object print 'finished executing release action' self.action_rec_flag = 0 # waiting for new action self.as_res.status = 0 #self.perched_count-=1 self.action_server.set_succeeded(self.as_res) else: pass rospy.sleep(rospy.Duration(0.05))