Hello,
I’m trying to drive a Rover by setting waypoints.
I’m using a development build of ArduRover, 3.4.0-dev, which fixed a timing bug between the PixHawk and IMU in order to use our visual odometry system. This is flashed on a PixHawk 2.1. I am using an offboard computer, a Jetson TX2, to publish MavRos messages and talk to the PixHawk.
This is a basic outline of what I’m doing:
- Launch MavRos
- Launch a node which arms the rover
- From the command line, I put the rover into GUIDED mode
- Using a controller, I publish to the MAVROS topic /mavros/setpoint_position/local which sends the MAVLINK message SET_POSITION_TARGET_LOCAL_NED.
What happens:
The rover does not respond to messages published to /mavros/setpoint_position/local. However, if I use Mission Planner and send a waypoint using “fly to here”, the rover spins up and starts moving.
What I’ve Tried:
I’ve gone through the MAVROS code and from my logs I’m almost certain the MAVLINK message SET_POSITION_TARGET_LOCAL_NED is being sent. The coordinate_frame and type_mask seem to be consistent with what I expect, and I’m seeing new x, y, z coordinates being dictated.
One thing I find strange is that I’m unable to set the Rover into GUIDED mode with my ROS node.
Below are code snippets of what I’m doing:
Initializing the Rover:
bool PX4Controller::APMRoverWaypoint::init(ros::NodeHandle& nh)
{
auto set_param_client = nh.serviceClient<mavros_msgs::ParamSet>("/mavros/param/set");
mavros_msgs::ParamSet param_set;
param_set.request.param_id = "SYSID_MYGCS";
const int gcs_id = 1;
param_set.request.value.integer = gcs_id;
if (set_param_client.call(param_set) && param_set.response.success)
ROS_INFO("(%s) Set SYSID_MYGCS to %d", getName().c_str(), gcs_id);
else
ROS_WARN("(%s) Failed to set SYSID_MYGCS to %d", getName().c_str(), gcs_id);
is_initialized_ = true;
return true;
}
Arming:
bool PX4Controller::arm()
{
// The setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(spin_rate_);
// Wait for FCU board connection
ROS_INFO("Waiting for FCU board...");
while (ros::ok() && fcu_state_.connected)
{
ros::spinOnce();
rate.sleep();
}
ROS_INFO("Connected to FCU board.");
geometry_msgs::PoseStamped goto_pose;
goto_pose.header.stamp = ros::Time::now();
goto_pose.header.frame_id = "map";
goto_pose.header.seq = 0;
goto_pose.pose.position.x = 0;
goto_pose.pose.position.y = 0;
goto_pose.pose.position.z = 0;
ROS_INFO("Getting current pose...");
for (int i = 100; ros::ok() && i > 0; --i)
{
ros::spinOnce();
rate.sleep();
geometry_msgs::PoseStamped current_pose = current_pose_; // current_pose_ is updated elsewhere, use its fixed time value for computations
goto_pose.pose.position.x = doExpSmoothing(current_pose.pose.position.x, goto_pose.pose.position.x, smoothing_factor_);
goto_pose.pose.position.y = doExpSmoothing(current_pose.pose.position.y, goto_pose.pose.position.y, smoothing_factor_);
goto_pose.pose.position.z = doExpSmoothing(current_pose.pose.position.z, goto_pose.pose.position.z, smoothing_factor_);
}
altitude_ = goto_pose.pose.position.z;
// Send a few setpoints before starting. Note that this will run before current
// position coordinates are obtained but that's ok as the vehicle is not armed/OFFBOARD yet.
ROS_INFO("Sending warmup messages...");
for (int i = 100; ros::ok() && i > 0; --i)
{
goto_pose.header.stamp = ros::Time::now();
goto_pose.header.frame_id = "map";
local_pose_pub_.publish(goto_pose);
ros::spinOnce();
rate.sleep();
}
mavros_msgs::SetMode offb_setmode;
offb_setmode.request.custom_mode = vehicle_->getOffboardModeName();
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
ros::Time init_start = ros::Time::now();
ROS_INFO("Switching to %s and arming...", vehicle_->getOffboardModeName().c_str());
while ( ros::ok() && (ros::Time::now() - init_start < ros::Duration(wait_for_arming_sec_)) )
{
geometry_msgs::PoseStamped current_pose = current_pose_; // current_pose_ is updated elsewhere, use its fixed time value for computations
if (fcu_state_.mode != vehicle_->getOffboardModeName() && (ros::Time::now() - last_request > ros::Duration(5.0)))
{
// Enable OFFBOARD mode
if (setmode_client_.call(offb_setmode) && offb_setmode.response.mode_sent)
{
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
}
else
{
if (!fcu_state_.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
{
// Arm FCU
if (arming_client_.call(arm_cmd) && arm_cmd.response.success)
{
ROS_INFO("Vehicle armed");
controller_state_ = ControllerState::Armed;
return true;
}
last_request = ros::Time::now();
}
else if(fcu_state_.armed)
{
ROS_INFO("Vehicle was already armed");
controller_state_ = ControllerState::Armed;
return true;
}
}
// Keep updating start coordinates until the vehicle is in OFFBOARD mode and armed.
// This prevents flying to 0,0,0 in some cases. goto_pose is initialized to real pose in the end
if (fcu_state_.mode != vehicle_->getOffboardModeName() || !fcu_state_.armed)
{
goto_pose.pose.position.x = doExpSmoothing(current_pose.pose.position.x, goto_pose.pose.position.x, smoothing_factor_);
goto_pose.pose.position.y = doExpSmoothing(current_pose.pose.position.y, goto_pose.pose.position.y, smoothing_factor_);
goto_pose.pose.position.z = doExpSmoothing(current_pose.pose.position.z, goto_pose.pose.position.z, smoothing_factor_);
}
goto_pose.header.stamp = ros::Time::now();
goto_pose.header.frame_id = "map";
local_pose_pub_.publish(goto_pose);
ros::spinOnce();
rate.sleep();
}
return false;
}
Spin and execute command which actually publishes the new pose to /mavros/setpoint_position/local:
void PX4Controller::spin()
{
if(controller_state_ != ControllerState::Armed)
{
ROS_INFO("Cannot spin in PX4Controller node! Vehicle is not armed!");
return;
}
// The setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(spin_rate_);
float distance = 0;
Eigen::Quaterniond orientation_quaternion;
Eigen::Matrix3d rotation_matrix;
Eigen::Vector3d euler_angles;
geometry_msgs::PoseStamped current_pose = current_pose_;
geometry_msgs::PoseStamped goto_pose = current_pose;
goto_pose.header.stamp = ros::Time::now();
goto_pose.header.frame_id = "map";
goto_pose.header.seq = 0;
ROS_INFO("PX4Controller is processing requests...");
while (ros::ok())
{
float linear_control_val = 0;
float angular_control_val = 0;
float yaw_control_val = 0;
float altitude_control_val = 0;
bool has_command = false;
// Get latest state and control inputs.
current_pose = current_pose_; // current_pose_ is updated elsewhere, use its fixed time value for computations
switch (controller_state_)
{
case ControllerState::Armed:
ROS_INFO("Armed mode.");
goto_pose.pose.position.z += takeoff_altitude_gain_;
controller_state_ = ControllerState::Takeoff;
ROS_INFO("Switching to Takeoff...");
break;
case ControllerState::Takeoff:
distance = getPoseDistance(current_pose, goto_pose);
ROS_INFO("Takeoff mode. Distance to end point = %f", distance);
if (distance <= position_tolerance_)
{
controller_state_ = ControllerState::Navigating;
is_moving_ = true;
altitude_ = current_pose.pose.position.z;
ROS_INFO("Switching to Navigate. Altitude: %f", altitude_);
}
break;
case ControllerState::Navigating:
// Log
tf::quaternionMsgToEigen(current_pose.pose.orientation, orientation_quaternion);
rotation_matrix = orientation_quaternion.toRotationMatrix();
euler_angles = rotation_matrix.eulerAngles(0,1,2);
ROS_DEBUG("NAVPOS: %4.2f, %4.2f, %4.2f, Att: %4.2f, %4.2f, %4.2f, SetAlt: %4.2f",
current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z,
angles::to_degrees(euler_angles[0]), angles::to_degrees(euler_angles[1]), angles::to_degrees(euler_angles[2]),
altitude_
);
if (fcu_state_.mode != vehicle_->getOffboardModeName())
{
// If offboard is off, don't move, just update goto_pose to be the current pose to avoid flyaway when offboard gets turned on!
ROS_INFO("Offboard is off. goto_pose will be the current_pose.");
goto_pose = current_pose;
break;
}
has_command = got_new_joy_command_ || got_new_dnn_command_;
if(!use_dnn_data_) // Use joystick/teleop control if DNN disabled
{
if(got_new_joy_command_)
{
linear_control_val = linear_control_val_;
angular_control_val = angular_control_val_;
yaw_control_val = yaw_control_val_;
altitude_control_val = altitude_control_val_;
got_new_joy_command_ = false;
}
}
else // Use DNN control if joystick is not touched
{
if(got_new_joy_command_ && (linear_control_val_!=0 || angular_control_val_!=0 ||
yaw_control_val_!=0 || altitude_control_val_!=0))
{
linear_control_val = linear_control_val_;
angular_control_val = angular_control_val_;
yaw_control_val = yaw_control_val_;
altitude_control_val = altitude_control_val_;
got_new_joy_command_ = false;
joy_commands_count_++;
}
else if(got_new_dnn_command_)
{
linear_control_val = dnn_linear_control_val_;
angular_control_val = dnn_angular_control_val_;
got_new_dnn_command_ = false;
dnn_commands_count_++;
}
else
{
// We get here only when there is no commands from DNN or no joystick movements outside of deadzone.
// Clearing the flag is currently requried only for rover.
has_command = false;
break;
}
}
// Log
ROS_DEBUG("NAVC: st=%d, l=%2.2f, a=%2.2f, y=%2.2f, alt=%2.2f",
(int)controller_state_, linear_control_val, angular_control_val,
yaw_control_val, altitude_control_val);
if(altitude_control_val != 0.0f)
{
altitude_ = altitude_ + 0.03f*altitude_control_val;
goto_pose.pose.position.z = altitude_;
}
if(yaw_control_val != 0)
{
// Rotating in place (left joystick)
angular_control_val = 0.3f*yaw_control_val;
linear_control_val = sqrtf(1 - angular_control_val*angular_control_val); // so linear and angular controls are still on a unit circle
// Use waypoint compute to compute direction, but turn in place!
geometry_msgs::Point face_new_point = computeNextWaypoint(current_pose, linear_control_val,
angular_control_val, 10.0 /*point at some distance*/);
goto_pose.pose.orientation = getRotationTo(current_pose.pose.position, face_new_point);
}
else // Moving (only right joystick)
{
if(linear_control_val == 0 && angular_control_val == 0)
{
if(is_moving_)
{
goto_pose.pose.orientation = current_pose.pose.orientation;
goto_pose.pose.position.z = altitude_;
goto_pose.pose.position.x = current_pose.pose.position.x;
goto_pose.pose.position.y = current_pose.pose.position.y;
is_moving_ = false;
}
}
else
{
is_moving_ = true;
// Compute next waypoint based on current commands
geometry_msgs::Point next_waypoint = computeNextWaypoint(current_pose, linear_control_val,
angular_control_val, linear_speed_);
next_waypoint.z = altitude_; // need to set Z so it holds the set altitude
goto_pose.pose.position = next_waypoint;
// Compute new orientation for the next waypoint if we are not going backwards or strafing
if(linear_control_val > 0)
{
goto_pose.pose.orientation = getRotationTo(current_pose.pose.position, next_waypoint);
}
}
}
break;
default:
ROS_ERROR("PX4Controller::spin detected unknown state: %d!", (int)controller_state_);
break;
}
goto_pose.header.stamp = ros::Time::now();
//ROS_INFO("Current mode is: %s", fcu_state_.mode.c_str());
vehicle_->executeCommand(*this, goto_pose, linear_control_val, angular_control_val, has_command);
// Log
tf::quaternionMsgToEigen(goto_pose.pose.orientation, orientation_quaternion);
rotation_matrix = orientation_quaternion.toRotationMatrix();
euler_angles = rotation_matrix.eulerAngles(0, 1, 2);
ROS_DEBUG("NAVGOTO: %4.2f, %4.2f, %4.2f, Att: %4.2f, %4.2f, %4.2f, SetAlt: %4.2f",
goto_pose.pose.position.x, goto_pose.pose.position.y, goto_pose.pose.position.z,
angles::to_degrees(euler_angles[0]), angles::to_degrees(euler_angles[1]), angles::to_degrees(euler_angles[2]),
altitude_);
ros::spinOnce();
rate.sleep();
}
}
void PX4Controller::APMRoverWaypoint::executeCommand(const PX4Controller& ctl, const geometry_msgs::PoseStamped& goto_pose,
float /*linear_control_val*/, float /*angular_control_val*/, bool /*has_command*/)
{
ROS_ASSERT(is_initialized_);
// Publish pose update to MAVROS
//ROS_INFO("PUBLISHING POSE!!!");
ctl.local_pose_pub_.publish(goto_pose);
}
I suspect I’m setting up something incorrectly, or I’m missing some flag that needs to be set.
I’ve set INITIAL_MODE to 15, and also set flight mode 4 to be GUIDED so that I can simply flip on switch on my RC transmitter. Echoing /mavros/state seems to be giving me the correct state:
header:
seq: 35
stamp:
secs: 1527118041
nsecs: 903583229
frame_id: ''
connected: True
armed: True
guided: True
mode: "GUIDED"
system_status: 4
Here is some logging I’ve done in MAVROS right before the SET_POSITION_TARGET_LOCAL_NED message is sent:
coordinate_frame: 1
type_mask 2552
SETTING POSITION TO: 0.00181769
0.468941
-0.0111004
SETTING VELOCITY TO: 0
0
0
POSITION: t[0.097 0.002 0.001] r[-0.013 -0.014 1.573]
The “type_mask” is logged as 2552, which translates to 0b100111111000. If I understand the MAVLINK docs correctly, this ignores everything except x, y, z, and yaw (http://mavlink.org/messages/common#SET_POSITION_TARGET_LOCAL_NED)
I’ve fiddled with some parameters in ArduRover itself, and I’ll list the ones that I think are relevant here:
INITIAL_MODE = 15
ARMING_REQUIRE = 0
ARMING_CHECK = 0
SYSID_THIS_MAV = 1
SYSID_MYCGS = 1
WP_RADIUS = 0.2
WP_OVERSHOOT = 0.2
SYSID_ENFORCE = 0
WP_SPEED = 1
I’ve also pulled a data flash log from my latest test run and attached it to this reply. It doesn’t seem like ArduRover is receiving commands to go to waypoints since I’m only seeing one or two GUID log-lines.
Binary file: https://drive.google.com/file/d/1rPLMVsDlza2j2HOYoKoVJecMAPZRKtRy/view?usp=sharing
Text file: https://drive.google.com/file/d/19hGCDtDo0j560ZYWEnh2VPsZAq4iNhh6/view?usp=sharing
Thanks!