| 119 | } |
| 120 | |
| 121 | void mainloop1() |
| 122 | { |
| 123 | int Control_Mode = 0; |
| 124 | int Move_mode = 0; |
| 125 | int Move_frame = 0; |
| 126 | int Trjectory_mode = 0; |
| 127 | float state_desired[4]; |
| 128 | Controller_Test Controller_Test; |
| 129 | |
| 130 | while(ros::ok()) |
| 131 | { |
| 132 | // Waiting for input |
| 133 | cout << ">>>>>>>>>>>>>>>> Welcome to use Prometheus Terminal Control <<<<<<<<<<<<<<<<"<< endl; |
| 134 | cout << "Please choose the Command.Mode: 0 for Idle, 1 for Takeoff, 2 for Hold, 3 for Land, 4 for Move, 5 for Disarm, 6 for User_Mode1, 7 for User_Mode2"<<endl; |
| 135 | cout << "Input 999 to switch to offboard mode and arm the drone (ONLY for simulation, please use RC in experiment!!!)"<<endl; |
| 136 | cin >> Control_Mode; |
| 137 | |
| 138 | if(Control_Mode == prometheus_msgs::ControlCommand::Move) |
| 139 | { |
| 140 | cout << "Please choose the Command.Reference_State.Move_mode: 0 for XYZ_POS, 1 for XY_POS_Z_VEL, 2 for XY_VEL_Z_POS, 3 for XYZ_VEL, 5 for TRAJECTORY"<<endl; |
| 141 | cin >> Move_mode; |
| 142 | |
| 143 | if(Move_mode == prometheus_msgs::PositionReference::TRAJECTORY) |
| 144 | { |
| 145 | cout << "For safety, please move the drone near to the trajectory start point firstly!!!"<<endl; |
| 146 | cout << "Please choose the trajectory type: 0 for Circle, 1 for Eight Shape, 2 for Step"<<endl; |
| 147 | cin >> Trjectory_mode; |
| 148 | cout << "Input the trajectory_total_time:"<<endl; |
| 149 | cin >> trajectory_total_time; |
| 150 | }else |
| 151 | { |
| 152 | cout << "Please choose the Command.Reference_State.Move_frame: 0 for ENU_FRAME, 1 for BODY_FRAME"<<endl; |
| 153 | cin >> Move_frame; |
| 154 | cout << "Please input the reference state [x y z yaw]: "<< endl; |
| 155 | cout << "setpoint_t[0] --- x [m] : "<< endl; |
| 156 | cin >> state_desired[0]; |
| 157 | cout << "setpoint_t[1] --- y [m] : "<< endl; |
| 158 | cin >> state_desired[1]; |
| 159 | cout << "setpoint_t[2] --- z [m] : "<< endl; |
| 160 | cin >> state_desired[2]; |
| 161 | cout << "setpoint_t[3] --- yaw [du] : "<< endl; |
| 162 | cin >> state_desired[3]; |
| 163 | } |
| 164 | }else if(Control_Mode == 999) |
| 165 | { |
| 166 | Command_to_pub.header.stamp = ros::Time::now(); |
| 167 | Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle; |
| 168 | Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1; |
| 169 | Command_to_pub.source = NODE_NAME; |
| 170 | Command_to_pub.Reference_State.yaw_ref = 999; |
| 171 | move_pub.publish(Command_to_pub); |
| 172 | } |
| 173 | |
| 174 | switch (Control_Mode) |
| 175 | { |
| 176 | case prometheus_msgs::ControlCommand::Idle: |
| 177 | Command_to_pub.header.stamp = ros::Time::now(); |
| 178 | Command_to_pub.Mode = prometheus_msgs::ControlCommand::Idle; |
no test coverage detected