Rectangular trajectory

Hello!!

I am testing offboard mode in my 3DR iris with pixhawk, I am making trajectories. I tested the MAVROS Offboard example and modified this code for a circular trajectory, the code works.

Now, I want to make a rectangular trajectory but I have had problems.

mavros_msgs::State current _state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}

int main(int argc, char **argv)
{
ros::init(argc, argv, “offb_node”);
ros::NodeHandle nh;

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
        ("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
        ("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
        ("mavros/set_mode");
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && current_state.connected){
    ros::spinOnce();
    rate.sleep();
}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0.5;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
    local_pos_pub.publish(pose);
    ros::spinOnce();
    rate.sleep();
}
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok()){
    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");
        }
        last_request = ros::Time::now();
    } else {
        if( !current_state.armed &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( arming_client.call(arm_cmd) &&
                arm_cmd.response.success){
                ROS_INFO("Vehicle armed");
            }
            last_request = ros::Time::now();
        }
    }
pose.pose.position.x = 1;
pose.pose.position.y = 0;
pose.pose.position.z = 1;
pose.pose.position.x = 0;
pose.pose.position.y = -1;
pose.pose.position.z = 1;
pose.pose.position.x = -1;
pose.pose.position.y = 0;
pose.pose.position.z = 1;
    local_pos_pub.publish(pose);
    ros::spinOnce();
    rate.sleep();
}
return 0;

}

When I do the simulation in gazebo, the iris always do the last waypoints:

pose.pose.position.x = -1;
pose.pose.position.y = 0;
pose.pose.position.z = 1;

But the iris does not make the previous waypoints.

I think my code is incorrect.

Can you give me some advice to solve my problem?

Thanks!!

When you publish pose it uses the latest values of pose so you need to set new pose and then publish. for example:-

pose.pose.position.x = 1;
pose.pose.position.y = 0;
pose.pose.position.z = 1;
local_pos_pub.publish(pose);


pose.pose.position.x = 0;
pose.pose.position.y = -1;
pose.pose.position.z = 1;
local_pos_pub.publish(pose);

pose.pose.position.x = -1;
pose.pose.position.y = 0;
pose.pose.position.z = 1;
local_pos_pub.publish(pose);