diff --git a/config/px4_config.yaml b/config/px4_config.yaml new file mode 100644 index 0000000..8a0d62f --- /dev/null +++ b/config/px4_config.yaml @@ -0,0 +1,270 @@ +# Common configuration for PX4 autopilot +# +# node: +startup_px4_usb_quirk: true + +# --- system plugins --- + +# sys_status & sys_time connection options +conn: + heartbeat_rate: 1.0 # send hertbeat rate in Hertz + timeout: 10.0 # hertbeat timeout in seconds + timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) + system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) + +# sys_status +sys: + min_voltage: 10.0 # diagnostics min voltage + disable_diag: false # disable all sys_status diagnostics, except heartbeat + +# sys_time +time: + time_ref_source: "fcu" # time_reference source + timesync_mode: MAVLINK + timesync_avg_alpha: 0.6 # timesync averaging factor + +# --- mavros plugins (alphabetical order) --- + +# 3dr_radio +tdr_radio: + low_rssi: 40 # raw rssi lower level for diagnostics + +# actuator_control +# None + +# command +cmd: + use_comp_id_system_control: false # quirk for some old FCUs + +# dummy +# None + +# ftp +# None + +# global_position +global_position: + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + rot_covariance: 99999.0 # covariance for attitude? + gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) + use_relative_alt: true # use relative altitude for local coordinates + tf: + send: false # send TF? + frame_id: "map" # TF frame_id + global_frame_id: "earth" # TF earth frame_id + child_frame_id: "base_link" # TF child_frame_id + +# imu_pub +imu: + frame_id: "base_link" + # need find actual values + linear_acceleration_stdev: 0.0003 + angular_velocity_stdev: 0.0003490659 // 0.02 degrees + orientation_stdev: 1.0 + magnetic_stdev: 0.0 + +# local_position +local_position: + frame_id: "map" + tf: + send: false + frame_id: "map" + child_frame_id: "base_link" + send_fcu: false + +# param +# None, used for FCU params + +# rc_io +# None + +# safety_area +safety_area: + p1: {x: 1.0, y: 1.0, z: 1.0} + p2: {x: -1.0, y: -1.0, z: -1.0} + +# setpoint_accel +setpoint_accel: + send_force: false + +# setpoint_attitude +setpoint_attitude: + reverse_thrust: false # allow reversed thrust + use_quaternion: true # enable PoseStamped topic subscriber + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "target_attitude" + rate_limit: 50.0 + +setpoint_raw: + thrust_scaling: 1.0 # used in setpoint_raw attitude callback. + # Note: PX4 expects normalized thrust values between 0 and 1, which means that + # the scaling needs to be unitary and the inputs should be 0..1 as well. + +# setpoint_position +setpoint_position: + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "target_position" + rate_limit: 50.0 + mav_frame: LOCAL_NED + +# setpoint_velocity +setpoint_velocity: + mav_frame: LOCAL_NED + +# vfr_hud +# None + +# waypoint +mission: + pull_after_gcs: true # update mission if gcs updates + +# --- mavros extras plugins (same order) --- + +# adsb +# None + +# debug_value +# None + +# distance_sensor +## Currently available orientations: +# Check http://wiki.ros.org/mavros/Enumerations +## +distance_sensor: + hrlv_ez4_pub: + id: 0 + frame_id: "hrlv_ez4_sonar" + orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing + field_of_view: 0.0 # XXX TODO + send_tf: true + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + lidarlite_pub: + id: 1 + frame_id: "lidarlite_laser" + orientation: PITCH_270 + field_of_view: 0.0 # XXX TODO + send_tf: true + sensor_position: {x: 0.0, y: 0.0, z: -0.1} + sonar_1_sub: + subscriber: true + id: 2 + orientation: PITCH_270 + laser_1_sub: + subscriber: true + id: 3 + orientation: PITCH_270 + +# image_pub +image: + frame_id: "px4flow" + +# fake_gps +fake_gps: + # select data source + use_mocap: true # ~mocap/pose + mocap_transform: false # ~mocap/tf instead of pose + use_vision: false # ~vision (pose) + # origin (default: Zürich) + geo_origin: + lat: 47.3667 # latitude [degrees] + lon: 8.5500 # longitude [degrees] + alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters] + eph: 2.0 + epv: 2.0 + satellites_visible: 5 # virtual number of visible satellites + fix_type: 3 # type of GPS fix (default: 3D) + tf: + listen: false + send: false # send TF? + frame_id: "map" # TF frame_id + child_frame_id: "fix" # TF child_frame_id + rate_limit: 10.0 # TF rate + gps_rate: 5.0 # GPS data publishing rate + +# landing_target +landing_target: + listen_lt: false + mav_frame: "LOCAL_NED" + land_target_type: "VISION_FIDUCIAL" + image: + width: 640 # [pixels] + height: 480 + camera: + fov_x: 2.0071286398 # default: 115 [degrees] + fov_y: 2.0071286398 + tf: + send: true + listen: false + frame_id: "landing_target" + child_frame_id: "camera_center" + rate_limit: 10.0 + target_size: {x: 0.3, y: 0.3} + +# mocap_pose_estimate +mocap: + # select mocap source + use_tf: false # ~mocap/tf + use_pose: true # ~mocap/pose + +# odom +odometry: + in: + frame_id: "odom" + child_frame_id: "base_link" + frame_tf: + local_frame: "local_origin_ned" + body_frame_orientation: "flu" + out: + frame_tf: + # available: check MAV_FRAME odometry local frames in + # https://mavlink.io/en/messages/common.html + local_frame: "vision_ned" + # available: ned, frd or flu (though only the tf to frd is supported) + body_frame_orientation: "frd" + +# px4flow +px4flow: + frame_id: "px4flow" + ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter + ranger_min_range: 0.3 # meters + ranger_max_range: 5.0 # meters + +# vision_pose_estimate +vision_pose: + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "vision_estimate" + rate_limit: 10.0 + +# vision_speed_estimate +vision_speed: + listen_twist: true # enable listen to twist topic, else listen to vec3d topic + twist_cov: true # enable listen to twist with covariance topic + +# vibration +vibration: + frame_id: "base_link" + +# wheel_odometry +wheel_odometry: + count: 2 # number of wheels to compute odometry + use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry + wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) + send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance) + send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry + frame_id: "map" # origin frame + child_frame_id: "base_link" # body-fixed frame + vel_error: 0.1 # wheel velocity measurement error 1-std (m/s) + tf: + send: true + frame_id: "map" + child_frame_id: "base_link" + +# vim:set ts=2 sw=2 et: diff --git a/config/px4_pluginlists.yaml b/config/px4_pluginlists.yaml new file mode 100644 index 0000000..f9087de --- /dev/null +++ b/config/px4_pluginlists.yaml @@ -0,0 +1,15 @@ +plugin_blacklist: +# common +- safety_area +# extras +- image_pub +- vibration +- distance_sensor +- rangefinder +- wheel_odometry +- mission +- tdr_radio +- fake_gps + +plugin_whitelist: [] +#- 'sys_*' diff --git a/frames.gv b/frames.gv deleted file mode 100644 index 3ab6af0..0000000 --- a/frames.gv +++ /dev/null @@ -1,9 +0,0 @@ -digraph G { -"local_origin" -> "local_origin_ned"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"]; -"fcu" -> "fcu_frd"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"]; -"fcu_frd" -> "fcu"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"]; -edge [style=invis]; - subgraph cluster_legend { style=bold; color=black; label ="view_frames Result"; -"Recorded at time: 124.428"[ shape=plaintext ] ; - }->"local_origin"; -} \ No newline at end of file diff --git a/frames.pdf b/frames.pdf deleted file mode 100644 index 455ad02..0000000 Binary files a/frames.pdf and /dev/null differ diff --git a/launch/cortex_bridge.launch b/launch/cortex_bridge.launch index ba8d79f..256d668 100644 --- a/launch/cortex_bridge.launch +++ b/launch/cortex_bridge.launch @@ -14,6 +14,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo pkg="oscillation_ctrl" type="MoCap_Localization_noTether.py" name="localize_node" + launch-prefix="xterm -e" /> @@ -21,4 +22,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo + + diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch index ac9b4e6..24bcb44 100644 --- a/launch/klausen_dampen.launch +++ b/launch/klausen_dampen.launch @@ -22,6 +22,11 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo name="linkStates_node" launch-prefix="xterm -e" /> + diff --git a/src/path_follow.cpp b/src/path_follow.cpp index fe2afea..855a614 100644 --- a/src/path_follow.cpp +++ b/src/path_follow.cpp @@ -5,8 +5,9 @@ #include #include -#include -#include +#include +#include +#include #include #include #include @@ -31,7 +32,7 @@ geometry_msgs::PoseStamped desPose; tf2::Quaternion q_cmd /*What I send*/, q_Jae; // What should be sent; double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = cmd geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg -offboard_ex::EulerAngles eulAng; +oscillation_ctrl::EulerAngles eulAng; mavros_msgs::AttitudeTarget thrust; @@ -42,12 +43,9 @@ void state_cb(const mavros_msgs::State::ConstPtr& msg){ } geometry_msgs::Quaternion q_des; -double alt_des; // Check desired height - // Cb to determine desired pose *Only needed for orientation void q_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ desPose = *msg; - alt_des = desPose.pose.position.z; q_des = desPose.pose.orientation; tf2::convert(q_des,q_cmd); // Convert msg type to tf2::quaternion type } @@ -83,7 +81,6 @@ void thrust_cb(const mavros_msgs::AttitudeTarget msg){ tf2::convert(thrust.orientation,q_Jae); // Convert from msg to quat } - int main(int argc, char **argv) { ros::init(argc, argv, "path_follow"); @@ -116,7 +113,7 @@ int main(int argc, char **argv) ("mavros/setpoint_raw/attitude",10); // Publish attitude cmds in euler angles - ros::Publisher euler_pub = nh.advertise + ros::Publisher euler_pub = nh.advertise ("command/euler_angles",10); // Service Clients @@ -126,6 +123,8 @@ int main(int argc, char **argv) ("mavros/set_mode"); ros::ServiceClient takeoff_cl = nh.serviceClient ("mavros/cmd/takeoff"); + ros::ServiceClient waypoint_cl = nh.serviceClient + ("status/waypoint_tracker"); //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms ros::Rate rate(20.0); @@ -140,18 +139,24 @@ int main(int argc, char **argv) /*********** Initiate variables ************************/ // Needed for attitude command mavros_msgs::AttitudeTarget attitude; - + attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO attitude.type_mask = 1|2|4; // Ignore body rates - // Retrieve parameters from Ctrl_param.yaml file - /*std::vector waypoints; - nh.getParam("sim/waypoints",waypoints); - - // Need to send pose commands until throttle has been established - buff_pose.pose.position.x = waypoints(1); - buff_pose.pose.position.y = waypoints(2); - buff_pose.pose.position.z = waypoints(3);*/ + // Retrieve desired waypoints + oscillation_ctrl::WaypointTrack wpoint_srv; + wpoint_srv.request.query = false; + if (waypoint_cl.call(wpoint_srv)) + { + ROS_INFO("Waypoints received"); + } + + // populate desired waypoints - this is only for original hover as + // a change of waypoints is handled by ref_signal.py + buff_pose.pose.position.x = wpoint_srv.response.xd.x; + buff_pose.pose.position.y = wpoint_srv.response.xd.y; + buff_pose.pose.position.z = wpoint_srv.response.xd.z; + double alt_des = buff_pose.pose.position.z; // Desired height // Desired mode is offboard mavros_msgs::SetMode offb_set_mode; diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index 889a931..371a399 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -8,22 +8,17 @@ import numpy as np import time import math +from pymavlink import mavutil from scipy import signal from scipy.integrate import odeint from oscillation_ctrl.msg import tethered_status, RefSignal - +from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest from controller_msgs.msg import FlatTarget - -from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point - -from geometry_msgs.msg import TwistStamped +from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped from sensor_msgs.msg import Imu - from mavros_msgs.msg import State -from pymavlink import mavutil - class Main: def __init__(self): @@ -35,6 +30,7 @@ class Main: self.tstart = rospy.get_time() # Keep track of the start time while self.tstart == 0.0: # Need to make sure get_rostime works self.tstart = rospy.get_time() + rospy.loginfo(self.tstart) self.dt = 1.0/rate # self.dt = 0.5 @@ -71,6 +67,33 @@ class Main: if rospy.has_param('sim/tether_length'): self.tetherL = rospy.get_param('sim/tether_length') # Tether length self.param_exists = True + elif rospy.get_time() - self.tstart >= 3.0: + self.tetherL = 0.0 + break + +# --------------------------------------------------------------------------------# +# SUBSCRIBERS # +# --------------------------------------------------------------------------------# + # Topic, msg type, and class callback method + rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb) + rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) + rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) + rospy.Subscriber('/mavros/state', State, self.state_cb) + +# --------------------------------------------------------------------------------# +# PUBLISHERS +# --------------------------------------------------------------------------------# + # Publish desired path to compute attitudes + self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10) + # Needed for geometric controller to compute thrust + self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10) + + # timer(s), used to control method loop freq(s) as defined by the rate(s) + self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) + +# --------------------------------------------------------------------------------# +# FEEDBACK AND INPUT SHAPING +# --------------------------------------------------------------------------------# # Smooth path variables self.EPS_F = np.zeros(9) # Final Epsilon/ signal @@ -79,7 +102,12 @@ class Main: # Constants for smooth path generation self.w_tune = 3.13 # 3.13 works well? ######################################################################### self.epsilon = 0.7 # Damping ratio - self.wn = math.sqrt(9.81/self.tetherL) + + # need exception if we do not have tether: + if self.tetherL == 0.0: + self.wn = self.w_tune + else: + self.wn = math.sqrt(9.81/self.tetherL) self.wd = self.wn*math.sqrt(1 - self.epsilon**2) self.k4 = 4*self.epsilon*self.w_tune self.k3 = ((2 + 4*self.epsilon**2)*self.w_tune**2)/self.k4 @@ -100,9 +128,9 @@ class Main: #elif rospy.get_time() - self.tstart >= 3.0: # self.xd = np.array([[0],[0],[5.0]]) # make our own if there are no waypoints - self.xd = Point() - self.get_xd = rospy.ServiceProxy('status/waypoint_tracker',WaypointTrack) - + #self.xd = Point() + self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack) + self.empty_point = Point() # Needed to query waypoint_server self.des_waypoints = True # True = changing waypoints @@ -111,10 +139,6 @@ class Main: self.y0 = [0, 0, 0, 0] self.z0 = [0, 0, 0, 0] -# --------------------------------------------------------------------------------# -# FEEDBACK AND INPUT SHAPING -# --------------------------------------------------------------------------------# - # Constants for feedback self.Gd = 0.325 self.td = 2*self.Gd*math.pi/self.wd @@ -148,30 +172,6 @@ class Main: self.sk = len(self.SF_delay_x[0]) # from Klausen 2017 self.ak = np.zeros(len(self.SF_delay_x[0])) self.s_gain = 0.0 # Gain found from sigmoid - -# --------------------------------------------------------------------------------# -# SUBSCRIBERS # -# --------------------------------------------------------------------------------# - # Topic, msg type, and class callback method - rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb) - rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb) - rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb) - rospy.Subscriber('/mavros/state', State, self.state_cb) - #rospy.Subscriber('/reference/waypoints',Position, self.waypoints_cb) ### Change to position - -# --------------------------------------------------------------------------------# -# PUBLISHERS -# --------------------------------------------------------------------------------# - # Publish desired path to compute attitudes - self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10) - # Needed for geometric controller to compute thrust - self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10) - - # timer(s), used to control method loop freq(s) as defined by the rate(s) - self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher) - -# ------------------------------------ _init_ ends ------------------------------ # - # --------------------------------------------------------------------------------# # CALLBACK FUNCTIONS # --------------------------------------------------------------------------------# @@ -230,13 +230,18 @@ class Main: pass def waypoints_srv_cb(self): - rospy.wait_for_service('status/waypoint_tracker') + rospy.wait_for_service('/status/waypoint_tracker') try: - self.xd = self.get_xd(True) -# if self.des_waypoints == True: - + resp = self.get_xd(False,self.empty_point) + self.xd = resp.xd except ValueError: pass + +################################################################# +#TODO Will need to add a function that gets a message from # +# a node which lets refsignal_gen.py know there has been a # +# change in xd and therefore runs waypoints_srv_cb again # +################################################################# # --------------------------------------------------------------------------------# # ALGORITHM @@ -322,9 +327,9 @@ class Main: else: print('No delay') - # Convolution and input shape filter, and feedback + # Convolution of shape filter and trajectory, and feedback def convo(self): - + self.waypoints_srv_cb() # needed for delay function # 1 = determine shapefilter array # 2 = determine theta/phi_fb @@ -332,9 +337,9 @@ class Main: feedBack = 2 # SOLVE ODE (get ref pos, vel, accel) - self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd[0],)) - self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd[1],)) - self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd[2],)) + self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,)) + self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,)) + self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,)) for i in range(1,len(self.y0)): self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i]) @@ -343,7 +348,7 @@ class Main: for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z - self.delay(j,shapeFil) # Determine the delay array + self.delay(j,shapeFil) # Determine the delay (shapefilter) array if self.SF_idx < len(self.SF_delay_x[0]): self.EPS_I[3*j] = self.x[1,j] @@ -363,9 +368,21 @@ class Main: # Populate EPS_F buffer with desired change based on feedback self.EPS_F[i] = self.EPS_I[i] + EPS_D[i] #+ EPS_D[i] -# self.x0 = [self.dr_pos.position.x, self.dr_vel.x, self.dr_acc.x, self.x[1,3]] -# self.y0 = [self.dr_pos.position.y, self.dr_vel.y, self.dr_acc.y, self.y[1,3]] -# self.z0 = [self.dr_pos.position.z, self.dr_vel.z, self.dr_acc.z, self.z[1,3]] + # Populate msg with epsilon_final + self.ref_sig.header.stamp = rospy.Time.now() + self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin + + self.ref_sig.position.x = self.EPS_F[0] + self.ref_sig.position.y = self.EPS_F[1] + self.ref_sig.position.z = self.EPS_F[2] + 0.5 + + self.ref_sig.velocity.x = self.EPS_F[3] + self.ref_sig.velocity.y = self.EPS_F[4] + self.ref_sig.velocity.z = self.EPS_F[5] + + self.ref_sig.acceleration.x = self.EPS_F[6] + self.ref_sig.acceleration.y = self.EPS_F[7] + self.ref_sig.acceleration.z = self.EPS_F[8] self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]] self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]] @@ -389,37 +406,31 @@ class Main: return EPS_D - def publisher(self,pub_tim): - - # Determine final ref signal - self.convo() - - # Populate msg with epsilon_final - self.ref_sig.position.x = self.EPS_F[0] - self.ref_sig.position.y = self.EPS_F[1] + def screen_output(self): - self.ref_sig.velocity.x = self.EPS_F[3] - self.ref_sig.velocity.y = self.EPS_F[4] - self.ref_sig.velocity.z = self.EPS_F[5] - - self.ref_sig.acceleration.x = self.EPS_F[6] - self.ref_sig.acceleration.y = self.EPS_F[7] - self.ref_sig.acceleration.z = self.EPS_F[8] - - # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin - self.ref_sig.type_mask = 2 - - # Publish command - self.ref_sig.header.stamp = rospy.Time.now() - self.pub_path.publish(self.ref_sig) - self.pub_ref.publish(self.ref_sig) - # Feedback to user rospy.loginfo(' Var | x | y | z ') rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) rospy.loginfo('_______________________') + + def publisher(self,pub_tim): + + # Determine final ref signal + self.convo() + + # Publish reference signal + self.pub_path.publish(self.ref_sig) + self.pub_ref.publish(self.ref_sig) + + # Give user feedback on published message: + rospy.loginfo(' Var | x | y | z ') + rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2]) + rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5]) + rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8]) + rospy.loginfo('_______________________') + if __name__=="__main__": diff --git a/src/square.py b/src/square.py index ef9e43f..73652b0 100755 --- a/src/square.py +++ b/src/square.py @@ -5,7 +5,7 @@ ### Script to generate set points which form a square with 2m side lengths import rospy, tf import time -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Point from std_msgs.msg import Bool class Main: @@ -13,11 +13,11 @@ class Main: def __init__(self): # variable(s) - self.Point = PoseStamped() + self.Point = Point() # Init x, y, & z coordinates - self.Point.pose.position.x = 0 - self.Point.pose.position.y = 0 - self.Point.pose.position.z = 3.5 + self.Point.x = 0 + self.Point.y = 0 + self.Point.z = 3.5 self.xarray = [1,2,2,2,1,0,0] self.yarray = [0,0,1,2,2,2,1] @@ -30,7 +30,7 @@ class Main: rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) # publisher(s), with specified topic, message type, and queue_size - self.pub_square = rospy.Publisher('/reference/waypoints', PoseStamped, queue_size = 5) + self.pub_square = rospy.Publisher('/reference/waypoints', Point, queue_size = 5) # rate(s) pub_rate = 1 # rate for the publisher method, specified in Hz @@ -49,28 +49,28 @@ class Main: else: # Check if i is too large. loop back to first point if self.i >= len(self.xarray): - self.Point.pose.position.x = 0 - self.Point.pose.position.y = 0 + self.Point.x = 0 + self.Point.y = 0 else: - self.Point.pose.position.x = self.xarray[self.i] - self.Point.pose.position.y = self.yarray[self.i] + self.Point.x = self.xarray[self.i] + self.Point.y = self.yarray[self.i] rospy.loginfo("Sending [Point x] %d [Point y] %d", - self.Point.pose.position.x, self.Point.pose.position.y) + self.Point.x, self.Point.y) # Published desired msgs - self.Point.header.stamp = rospy.Time.now() + # self.Point.header.stamp = rospy.Time.now() self.pub_square.publish(self.Point) self.j += 1 self.i = self.j // self.buffer # self.Point.header.stamp = rospy.Time.now() -# self.Point.pose.position.x = self.xarray[self.i] -# self.Point.pose.position.y = self.yarray[self.i] +# self.Point.x = self.xarray[self.i] +# self.Point.y = self.yarray[self.i] # rospy.loginfo("Sending [Point x] %d [Point y] %d", -# self.Point.pose.position.x, self.Point.pose.position.y) +# self.Point.x, self.Point.y) # Published desired msgs # self.pub_square.publish(self.Point) diff --git a/src/step.py b/src/step.py index e229107..1301d0c 100755 --- a/src/step.py +++ b/src/step.py @@ -14,9 +14,9 @@ class Main: # variable(s) self.Point = Point() # Init x, y, & z coordinates - self.Point.pose.position.x = 1 - self.Point.pose.position.y = 0 - self.Point.pose.position.z = 4.0 + self.Point.x = 1 + self.Point.y = 0 + self.Point.z = 4.0 self.bool = False @@ -41,13 +41,13 @@ class Main: if self.bool == False: rospy.loginfo('Waiting...') else: - self.Point.header.stamp = rospy.Time.now() + #self.Point.header.stamp = rospy.Time.now() # Published desired msgs self.pub_step.publish(self.Point) rospy.loginfo("Sending [Point x] %d [Point y] %d", - self.Point.pose.position.x, self.Point.pose.position.y) + self.Point.x, self.Point.y) if __name__=="__main__": diff --git a/src/wpoint_tracker.py b/src/wpoint_tracker.py index 0a7dcd4..831edf1 100755 --- a/src/wpoint_tracker.py +++ b/src/wpoint_tracker.py @@ -36,6 +36,7 @@ class Main: self.xd.x = 0.0 self.xd.y = 0.0 self.xd.z = 5.0 + break # service(s) @@ -47,7 +48,12 @@ class Main: def waypoint_relay(self,req): - self.resp.xd = self.xd + #TODO Need to add check to make sure query is boolean + if req.query: + self.xd = req.xd + self.resp.xd = self.xd + else: + self.resp.xd = self.xd return self.resp # Change desired position if there is a change @@ -63,7 +69,9 @@ if __name__=="__main__": rospy.init_node('waypoints_server',anonymous=False) try: obj = Main() # create class object - s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay) + s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay) + rospy.loginfo_once('waypoints_server has started with:\nxd.x = %.1f\nxd.y = %.1f\nxd.z = %.1f', obj.xd.x, obj.xd.y, obj.xd.z) + rospy.spin() # loop until shutdown signal except rospy.ROSInterruptException: diff --git a/srv/WaypointTrack.srv b/srv/WaypointTrack.srv index 7352ec2..b9502db 100644 --- a/srv/WaypointTrack.srv +++ b/srv/WaypointTrack.srv @@ -1,4 +1,5 @@ -bool query # bool to request waypoints +bool query # bool to request or set waypoints +geometry_msgs/Point xd # waypoints --- geometry_msgs/Point xd # waypoints - +