• Non ci sono risultati.

Velocity control with MAVROS and MAVLink

MAVROS velocity control procedure and final results

6.1 Velocity control with MAVROS and MAVLink

After these preliminary steps have been completed, the real control procedure can be started by connecting the Pixhawk to the OpenCR1.0 board of the rover and to the radio transmitter that will be in communication with the companion computer.

At this point the autopilot is not armed and is set on HOLD mode that does not allow the control with a MAVROS console. In order to prepare the Pixhawk and have the full control of the inputs the following instructions shall be executed in the order in which they are indicated:

• roscore: as explained in Chapter 1, when working in a ROS environment, this command shall be always the first to be executed.

• roslaunch mavros apm.launch fcu_url:="/dev/ttyUSB0:57600": with this command the MAVROS environment is initialized and the Pixhawk begins to exchange heartbeats messages via MAVLink and waits for a mission. Moreover, the desired baud rate for the communication is also spelt out and given to the fcu_url parameter of the launch file.

• rosrun mavros mavsys mode -c GUIDED: with this instruction the MODE parameter is changed to GUIDED, allowing to have full control of the PWM outputs of the autopilot that means being able to control the motors of the rover via MAVROS console.

• rosrun mavros mavsafety arm: this command is crucial since it arms the vehicle, allowing the velocity control of its motors. If the vehicle is not armed a lot of features will be forbidden to use and some parameters modification wont be available for the changing.

At this point it is possible to write a script to manage the values sent to the /mavros/setpoint_velocity/cmd_vel topic that is the topic utilized to write nu-merical values in a range from -1 to 1 that will be remapped and traduced into PWM values that will be then generated by the autopilot and found as outputs in the main pins of the Pixhawk. For this purpose, the following script has been used:

1 # include <ros / ros .h>

2 # include < geometry_msgs / TwistStamped .h>

3

4 int main ( int argc , charargv [])

5 {

6 ros :: init ( argc , argv , " cmd_vel_fusion ");

7 ros :: NodeHandle nh;

8 ros :: Publisher send_velocity_pub = nh . advertise < geometry_msgs :: TwistStamped >( "/ mavros / setpoint_velocity / cmd_vel " , 1000) ;

9 ros :: Rate loop_rate (100) ; 10

6 – MAVROS velocity control procedure and final results

11 geometry_msgs :: TwistStamped send_velocity_msg ; 12

13 double ros_roll =0.0;

14 double ros_pitch =0.0;

15 double ros_yaw =0.0;

16 double ros_throttle =0.0;

17 int count = 1;

18

19 while ( ros :: ok ()) 20 {

21 nh . param < double >( " ros_roll " , ros_roll , 0.0) ; 22 nh . param < double >( " ros_pitch " , ros_pitch , 0.0) ; 23 nh . param < double >( " ros_yaw " , ros_yaw , 0.0) ;

24 nh . param < double >( " ros_throttle " , ros_throttle ,0.0) ; 25

26 send_velocity_msg . header . stamp = ros :: Time :: now () ; 27 send_velocity_msg . header . seq = count ;

28 send_velocity_msg . header . frame_id = 1 ; 29

30 send_velocity_msg . twist . linear .x = ros_throttle ; 31 send_velocity_msg . twist . linear .y = 0.0;

32 send_velocity_msg . twist . linear .z = 0.0;

33 send_velocity_msg . twist . angular .x = ros_pitch ; 34 send_velocity_msg . twist . angular .y = ros_roll ; 35 send_velocity_msg . twist . angular .z = ros_yaw ; 36

37 send_velocity_pub . publish ( send_velocity_msg );

38 ros :: spinOnce () ; 39 count ++;

40 loop_rate . sleep () ; 41 }

42 return 0;

43 }

From line 6 to 9 the node handler and the publisher are initialized. In particular, this script publishes messages on /mavros/setpoint_velocity/cmd_vel topic with a rate of 100 Hz (ros::Rate loop_rate(100)) .

From line 13 to line 17 the variables used in the code are initialized.

From line 21 to 24 the parameters are stored within the ROS parameter server using the node handler created in line 7. This allows the usage of rosparam command with which it is possible to change the values of these parameters from the terminal while the script is running.

From line 26 to 28 there are some info to be printed on the terminal while this script is running.

6 – MAVROS velocity control procedure and final results

From line 30 to 35 the velocity variables assume the values passed with the rosparam command. In particular, the two variables that are used for this application are ros_throttle that manages the linear.x speed, and ros_yaw that manages the an-gular.z velocity.

Finally, from line 37 to 40 the publisher is set and the ros.spin loop is implemented in order to be able to change the values for the inputs at any time.

While this whole procedure is being performed, there are two topics that shall be always kept in check because of the information they provide. The first one is /mavros/s-tate, a topic that allow to check if the autopilot is connected, armed and which kind of mode is active for the current application.

The second one, on the other hand, is /mavros/setpoint_velocity/cmd_vel itself, the topic used to write the values for the input commands. This control can be done by invoking the rostopic echo command, followed by the name of the topic that the user wants to check.

The two terminals used to check /mavros/state topic and /mavros/setpoint_velocity/

cmd_veltopic are reported below in figure 6.1 and in figure 6.2 , respectively:

Figure 6.1: mavros/state topic echo terminal

6 – MAVROS velocity control procedure and final results

Figure 6.2: mavros/setpoint_velocity/cmd_vel topic echo terminal

As it is shown in figure 6.2, initially the values for linear.x and angular.z velocities are set to zero. In order to change these values, the rosparam command has been used with the following syntax:

1 rosparam set ros_throttle 1 2 rosparam set ros_yaw 1

After these commands, the new values for the ros_throttle and ros_yaw param-eters have been updated, as it is shown in the echo terminal of the /mavros/set-point_velocity/cmd_vel topic reported below :

Figure 6.3: Updated throttle and yaw values after the usage of rosparam command

6 – MAVROS velocity control procedure and final results

Documenti correlati