How to use the simple pd position controller node in the ROS wrapper?

[abdelhakim96]

How to use the simple pd position controller node in the ROS wrapper?

[xxEoD2242]

Hey @abdelhakim96,

Some suggestions for you:

1. You will need to update the odometry and velocity topics in the main pd controller to have the name of your drone. I am working on rewriting that to pull the name of your drone from the settings file but have a lot of work at the moment. The file is pd_position_controller_simple.cpp.

void PIDPositionController::initialize_ros()
{
    vel_cmd_ = airsim_ros_pkgs::VelCmd();
    // ROS params
    double update_control_every_n_sec;
    nh_private_.getParam("update_control_every_n_sec", update_control_every_n_sec);

    // ROS publishers
    airsim_vel_cmd_world_frame_pub_ = nh_private_.advertise<airsim_ros_pkgs::VelCmd>("/airsim_node/PX4/vel_cmd_world_frame", 1);
 
    // ROS subscribers
    airsim_odom_sub_ = nh_.subscribe("/airsim_node/PX4/odom_local_ned", 50, &PIDPositionController::airsim_odom_cb, this);
    home_geopoint_sub_ = nh_.subscribe("/airsim_node/home_geo_point", 50, &PIDPositionController::home_geopoint_cb, this);
    // todo publish this under global nodehandle / "airsim node" and hide it from user
    local_position_goal_srvr_ = nh_.advertiseService("/airsim_node/local_position_goal", &PIDPositionController::local_position_goal_srv_cb, this);
    local_position_goal_override_srvr_ = nh_.advertiseService("/airsim_node/local_position_goal/override", &PIDPositionController::local_position_goal_srv_override_cb, this);
    gps_goal_srvr_ = nh_.advertiseService("/airsim_node/gps_goal", &PIDPositionController::gps_goal_srv_cb, this);
    gps_goal_override_srvr_ = nh_.advertiseService("/airsim_node/gps_goal/override", &PIDPositionController::gps_goal_srv_override_cb, this);

    // ROS timers
    update_control_cmd_timer_ = nh_private_.createTimer(ros::Duration(update_control_every_n_sec), &PIDPositionController::update_control_cmd_timer_cb, this);
}

2. This uses services in ROS. You can write your own code to give x,y,z,yaw messages in the format given in the srv/ folder for things like land, takeoff, position, etc. Here is some reference code I wrote just to see what this could do. It's still a work in progress.

#!/usr/bin/env python3

import rospy
from airsim_ros_pkgs.srv import SetLocalPosition, Takeoff, Land

if __name__ == '__main__':
    rospy.init_node('px4_move_control')
    parent_frame = rospy.get_param('~parent_frame', 'world')
    child_frame = rospy.get_param('~child_frame','drone_name')
    drone_name = rospy.get_param('~drone_name', 'drone_name')
    odom_topic = rospy.get_param('~odom_topic','/airsim_node/{drone_name}/odom_local_ned'.format(drone_name=drone_name))
    goal_topic = rospy.get_param('~goal_topic', "/airsim_node/local_position_goal")
    service_name = '/airsim_node/local_position_goal'
    takeoff_name = '/airsim_node/{drone_name}/takeoff'.format(drone_name=drone_name)
    land_name = '/airsim_node/{drone_name}/land'.format(drone_name=drone_name)
    rospy.wait_for_service(service_name)
    rospy.wait_for_service(takeoff_name)
    rospy.wait_for_service(land_name)
    set_local_position = rospy.ServiceProxy(service_name, SetLocalPosition)
    takeoff = rospy.ServiceProxy(takeoff_name, Takeoff)
    land = rospy.ServiceProxy(land_name, Land)
    taken_off = False
    in_the_air = False


    rate = rospy.Rate(30.0)
    try:
        while not rospy.is_shutdown():
            
                if not in_the_air:
                    while not taken_off:
                        try:
                            # This calls the service with a message strucutre that is defined
                            # in the SetLocalPosition.srv
                            taken_off = takeoff(True)
                            print("The vehicle has taken off!")
                            in_the_air = True
                        except rospy.ServiceException as exc:
                            print("Service did not process request: " + str(exc))
                
                
                print("Move the drone by x, y, z (meters) and yaw (degrees).\n")
                input_valid = False
                while not input_valid:
                    try:
                        x_pos = input("X Position: \t")
                        assert isinstance(float(x_pos), float)
                        y_pos = input("Y Position: \t")
                        assert isinstance(float(y_pos), float)
                        z_pos = input("Z Position: \t")
                        assert isinstance(float(z_pos), float)
                        yaw = input("Yaw: \t")
                        assert isinstance(float(yaw), float)
                        input_valid = True
                    except AssertionError:
                        print("This input was not correct!")

                print("Goal input is valid: \n\n Executing...")
                # TODO: Make a custom message structure to handle this
                # goal = SetLocalPosition()
                x_pos = float(x_pos)
                y_pos = float(y_pos)
                z_pos = float(z_pos)
                yaw = float(yaw)
                #goal.vehicle_name = child_frame

                try:
                    # This calls the service with a message strucutre that is defined
                    # in the SetLocalPosition.srv
                    response = set_local_position(x_pos, y_pos, z_pos, yaw, child_frame)
                except rospy.ServiceException as exc:
                    print("Service did not process request: " + str(exc))
                
                print("Moving Vehicle!")
                rate.sleep()
    except Exception:
            print("There was an exception!")
    finally:
        if in_the_air:
            try:
                # This calls the service with a message strucutre that is defined
                # in the SetLocalPosition.srv
                resp1 = land(True)
            except rospy.ServiceException as exc:
                print("Service did not process request: " + str(exc))

Put this in a python file in the scripts folder of airsim_ros_pkgs.

Then, add the following to your launch file:

<launch>
	<arg name="output" default="log"/>
	<arg name="publish_clock" default="false"/>
	<arg name="is_vulkan" default="true"/>
	<arg name="host" default="localhost" />

	<node name="airsim_node" pkg="airsim_ros_pkgs" type="airsim_node" output="$(arg output)">
		<param name="is_vulkan" type="bool" value="false" /> 
		<!-- ROS timer rates. Note that timer callback will be processed at maximum possible rate, upperbounded by the following ROS params -->
		<param name="update_airsim_img_response_every_n_sec" type="double" value="0.05" /> 
		<param name="update_airsim_control_every_n_sec" type="double" value="0.01" />
		<param name="update_lidar_every_n_sec" type="double" value="0.01" />
		<param name="publish_clock" type="bool" value="$(arg publish_clock)" />
		<param name="host_ip" type="string" value="$(arg host)" />
		<param name="world_frame_id" type="string" value="world_ned" />
	</node>

    <!-- Vehicle Dynamic Constraints -->
	<include file="$(find airsim_ros_pkgs)/launch/dynamic_constraints.launch"/>
    <!-- Simple PID Position Controller -->
	<include file="$(find airsim_ros_pkgs)/launch/position_controller_simple.launch"/>

    <node name="px4_move_control" pkg="airsim_ros_pkgs" type="px4_move_control.py" output="screen">
        <!-- TODO: PUll this from the settings file -->
        <param name="drone_name" type="string" value="PX4" />
    </node>

	<!-- Static transforms -->
	<include file="$(find airsim_ros_pkgs)/launch/static_transforms.launch"/>
</launch>

Once you do this, you will be able to run the airsim node, position controller and this node, which will prompt you for x,y,z and yaw. Beware that this is in NED coordinates, so give - values for z or just modify the python code.

[abdelhakim96]

Awesome! Thank you very much @xxEoD2242

[leoborgnino]

Hey @abdelhakim96,

Some suggestions for you:

1. You will need to update the odometry  and velocity topics in the main pd controller to have the name of your drone. I am working on rewriting that to pull the name of your drone from the settings file but have a lot of work at the moment. The file is `pd_position_controller_simple.cpp`.

@xxEoD2242 You can look here, there are a few approaches related to that:
#2789