diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch index 0e8ff7c..3baf0ef 100644 --- a/launch/klausen_dampen.launch +++ b/launch/klausen_dampen.launch @@ -42,7 +42,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo type="klausen_control.py" name="klausenCtrl_node" output="screen" - /> ("mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient - ("mavros/set_mode"); + ("mavros/set_mode"); ros::ServiceClient takeoff_cl = nh.serviceClient ("mavros/cmd/takeoff"); - ros::ServiceClient waypoint_cl = nh.serviceClient + ros::ServiceClient waypoint_cl = nh.serviceClient ("status/waypoint_tracker"); //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms @@ -132,31 +132,39 @@ int main(int argc, char **argv) // wait for FCU connection while(ros::ok() && !current_state.connected){ ros::spinOnce(); - ROS_INFO("Waiting for FCU connection"); + ROS_INFO("Waiting for FCU connection"); rate.sleep(); } + + if (current_state.connected){ + ROS_INFO("Connected to FCU"); + } else { + ROS_INFO("Never Connected"); + } /*********** Initiate variables ************************/ + //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms + ros::Rate rate_pub(75.0); // 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 desired waypoints - oscillation_ctrl::WaypointTrack wpoint_srv; - wpoint_srv.request.query = false; - if (waypoint_cl.call(wpoint_srv)) - { - ROS_INFO("Waypoints received"); - } + // 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 + // 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 + double alt_des = buff_pose.pose.position.z; // Desired height // Desired mode is offboard mavros_msgs::SetMode offb_set_mode; @@ -177,6 +185,7 @@ int main(int argc, char **argv) for(int i = 100; ros::ok() && i > 0; --i){ local_pos_pub.publish(buff_pose); ros::spinOnce(); + ROS_INFO("Publishing position setpoints"); rate.sleep(); } @@ -199,21 +208,16 @@ int main(int argc, char **argv) eulAng.commanded = K_ang; eulAng.desired = J_ang; - if(current_state.mode != "OFFBOARD" && - (ros::Time::now() - last_request > ros::Duration(5.0))){ - if( set_mode_client.call(offb_set_mode) && - offb_set_mode.response.mode_sent){ - ROS_INFO("Offboard enabled"); + if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ + if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ } else { - ROS_INFO("Could not enter offboard mode"); + ROS_INFO("Could not enter offboard mode"); } - last_request = ros::Time::now(); + //last_request = ros::Time::now(); } else { - if( !current_state.armed && - (ros::Time::now() - last_request > ros::Duration(3.0))){ - if( arming_client.call(arm_cmd) && - arm_cmd.response.success){ - ROS_INFO("Vehicle armed"); + if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){ + if( arming_client.call(arm_cmd) && arm_cmd.response.success){ + ROS_INFO("Vehicle armed"); } last_request = ros::Time::now(); } @@ -229,17 +233,17 @@ int main(int argc, char **argv) attitude.thrust = thrust.thrust; // Determine which messages to send - if(ros::Time::now() - tkoff_req > ros::Duration(22.0) && takeoff){ + if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){ attitude.orientation = q_des; attitude.header.stamp = ros::Time::now(); att_targ.publish(attitude); att_cmds = true; - ROS_INFO("Attitude Ctrl"); + ROS_INFO("ATTITUDE CTRL"); ROS_INFO("Des Altitude: %.2f", alt_des); ROS_INFO("Cur Altitude: %.2f", current_altitude); } else { local_pos_pub.publish(buff_pose); - ROS_INFO("Position Ctrl"); + ROS_INFO("POSITION CTRL"); ROS_INFO("Des Altitude: %.2f", alt_des); ROS_INFO("Cur Altitude: %.2f", current_altitude); } @@ -248,7 +252,7 @@ int main(int argc, char **argv) euler_pub.publish(eulAng); ros::spinOnce(); - rate.sleep(); + rate_pub.sleep(); } } diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index 631242a..ba2365b 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -324,12 +324,12 @@ class Main: # SOLVE ODE (get ref pos, vel, accel) 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,)) + #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]) self.y[:,i] = np.clip(self.y[:,i], a_min = -self.max[i], a_max = self.max[i]) - self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i]) + #self.z[:,i] = np.clip(self.z[:,i], a_min = -self.max[i], a_max = self.max[i]) for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z @@ -338,11 +338,11 @@ class Main: if self.SF_idx < len(self.SF_delay_x[0]): self.EPS_I[3*j] = self.x[1,j] self.EPS_I[3*j+1] = self.y[1,j] - self.EPS_I[3*j+2] = self.z[1,j] + #self.EPS_I[3*j+2] = self.z[1,j] else: self.EPS_I[3*j] = self.A1*self.x[1,j] + self.A2*self.SF_delay_x[j,0] # Determine convolution (x) self.EPS_I[3*j+1] = self.A1*self.y[1,j] + self.A2*self.SF_delay_y[j,0] # Determine convolution (y) - self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim + #self.EPS_I[3*j+2] = self.z[1,j] # No need to convolute z-dim self.delay(1,feedBack) # Detemine feedback array @@ -358,18 +358,22 @@ class Main: 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] - self.ref_sig.position.z = 5.0 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] + + # Do not need to evaluate z + self.ref_sig.position.z = 5.0 + self.ref_sig.velocity.z = 0 + self.ref_sig.acceleration.z = 0 + #self.ref_sig.position.z = self.EPS_F[2] + #self.ref_sig.velocity.z = self.EPS_F[5] + #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]] - self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]] + #self.z0 = [self.dr_pos.position.z, self.z[1,1], self.z[1,2], self.z[1,3]] self.SF_idx += 1 self.FB_idx += 1 @@ -410,7 +414,7 @@ class Main: self.pub_ref.publish(self.ref_sig) # Give user feedback on published message: - self.screen_output() + #self.screen_output() if __name__=="__main__":