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!!