原文链接: https://pixhawk.org/dev/ros/ground_rover
This tutorial will show how to use the pixhawk to control a ground rover via ROS.
example for extras.txt:
usleep 100000 #start mavlink custom stream on telem2 (/dev/ttyS2 internally) with 921600 and ftp enabled (-x) mavlink start -m custom -d /dev/ttyS2 -b 921600 -x usleep 100000 mavlink stream -d /dev/ttyS2 -s HEARTBEAT -r 5 usleep 100000 mavlink stream -d /dev/ttyS2 -s STATUSTEXT -r 1 usleep 100000 mavlink stream -d /dev/ttyS2 -s SYS_STATUS -r 1 usleep 100000 mavlink stream -d /dev/ttyS2 -s PARAM_VALUE -r 200 usleep 100000 mavlink stream -d /dev/ttyS2 -s HIGHRES_IMU -r 50 usleep 100000 mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 usleep 100000 mavlink stream -d /dev/ttyS2 -s ATTITUDE_QUATERNION -r 50 usleep 100000 mavlink stream -d /dev/ttyS2 -s VFR_HUD -r 10 usleep 100000 mavlink stream -d /dev/ttyS2 -s GPS_RAW_INT -r 5 usleep 100000 mavlink stream -d /dev/ttyS2 -s GLOBAL_POSITION_INT -r 5 usleep 100000 mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 5
roslaunch mavros px4.launch
In this tutorial we will use the mavros actuator_control plugin to send direct servo commands to the FCU for steering and throttle.
example for a simple ros publisher:
/* Copyright Marcel Stüttgen 2015 <[email protected]> simple ros node that will publish ActuatorControl message to /mavros/actuator_controls */ #include <ros/ros.h> #include <mavros/ActuatorControl.h> int main(int argc, char *argv[]) { ros::init(argc, argv, "actuator_controls_publisher"); ros::NodeHandle nh; ros::Publisher actuator_controls_pub = nh.advertise<mavros::ActuatorControl>("/mavros/actuator_controls", 1000); ros::Rate loop_rate(10); double ros_roll; double ros_pitch; double ros_yaw; double ros_throttle; while (ros::ok()) { mavros::ActuatorControl actuator_control_msg; nh.param<double>("ros_roll", ros_roll, 0.0); nh.param<double>("ros_pitch", ros_pitch, 0.0); nh.param<double>("ros_yaw", ros_yaw, 0.0); nh.param<double>("ros_throttle", ros_throttle, 0.0); /*send to control group 0: 0:roll 1:pitch 2:yaw 3:throttle 4:flaps 5:spoilers 6:airbrakes 7:landing gear see https://pixhawk.ethz.ch/mavlink/#SET_ACTUATOR_CONTROL_TARGET and https://pixhawk.org/dev/mixing */ actuator_control_msg.header.stamp = ros::Time::now(); actuator_control_msg.group_mix = 0; actuator_control_msg.controls[0] = ros_roll; actuator_control_msg.controls[1] = ros_pitch; actuator_control_msg.controls[2] = ros_yaw; actuator_control_msg.controls[3] = ros_throttle; actuator_control_msg.controls[4] = 0.0; actuator_control_msg.controls[5] = 0.0; actuator_control_msg.controls[6] = 0.0; actuator_control_msg.controls[7] = 0.0; actuator_controls_pub.publish(actuator_control_msg); ros::spinOnce(); loop_rate.sleep(); } return 0; }
rosrun mavros mavsys mode -c OFFBOARD
rosrun mavros mavsafety arm
throttle:
rosparam set ros_throttle [-1:1]
steering:
rosparam set ros_yaw [-1:1]
* Video (first autonomous drive with ROS move_base)
* Video (autonomous driving in our laboratory with move_base and sbpl_lattice planner)