wellton/ardrone_autonomy
1
0
Fork 0
Código Issues Pull Requests Ações Pacotes Projetos Versões Wiki Atividade

Manually merging #25 to add magnetometer topic (by @sameerparekh)

Ver Código-Fonte
Esse commit está contido em:
Mani Monajjemi
2012-11-09 18:37:23 -08:00
commit de autolab
pai 0b58364a5c
commit 0dc6a2a84b
3 arquivos alterados com 82 adições e 53 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
README.md
+8 -1
Ver Arquivo
@@ -6,6 +6,7 @@
### Updates
- *November 9 2012*: Critical Bug in sending configurations to drone fixed ([More info](https://github.com/AutonomyLab/ardrone_autonomy/issues/24)). Seperate topic for magnetometer data added ([More info](https://github.com/AutonomyLab/ardrone_autonomy/pull/25)).
- *September 5 2012*: Experimental automatic IMU bias removal.
- *August 27 2012*: Thread-safe SDK data access. Synchronized `navdata` and `camera` topics.
- *August 20 2012*: The driver is now provides ROS standard camera interface.
@@ -91,6 +92,10 @@ Information received from the drone will be published to the `ardrone/navdata` t
The linear acceleration, angular velocity and orientation from the `Navdata` is also published to a standard ROS [`sensor_msgs/Imu`](http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html) message. The units are all metric and the reference frame is in `Base` frame. This topic is experimental. The covariance values are specified by specific parameters.
### Megnetometer Data
The normalized magnetometer readings are also published to `ardrone/mag` topic as a standard ROS [`geometry_msgs/Vector3Stamped`](http://www.ros.org/doc/api/geometry_msgs/html/msg/Vector3Stamped.html) message.
### Cameras
Both AR-Drone 1.0 and 2.0 are equipped with two cameras. One frontal camera pointing forward and one vertical camera pointing downward. This driver will create three topics for each drone: `ardrone/image_raw`, `ardrone/front/image_raw` and `ardrone/bottom/image_raw`. Each of these three are standard [ROS camera interface](http://ros.org/wiki/camera_drivers) and publish messages of type [image transport](http://www.ros.org/wiki/image_transport). The driver is also a standard [ROS camera driver](http://www.ros.org/wiki/camera_drivers), therefor if camera calibration information is provided either as a set of ROS parameters or appropriate `ardrone_front.yaml` and/or `ardrone_bottom.yaml`, the information will be published in appropriate `camera_info` topics. Please check the FAQ section for more information.
@@ -208,9 +213,11 @@ The Parrot's license, copyright and disclaimer for `ARDroneLib` are included wit
## Contributors
- [Rachel Brindle](https://github.com/younata) - [Enhanced Navdata for AR-Drone 2.0](https://github.com/AutonomyLab/ardrone_autonomy/pull/2)
- [Mike Hamer](https://github.com/mikehamer) - Added support for proper SDK2 way of configuring the Drone via parameter (critical bug fix). [More Info](https://github.com/AutonomyLab/ardrone_autonomy/pull/26)
- [Sameer Parekh](https://github.com/sameerparekh) - [Seperate Magnetometer Topic](https://github.com/AutonomyLab/ardrone_autonomy/pull/25)
- [Devmax](https://github.com/devmax) - [Flat Trim](https://github.com/AutonomyLab/ardrone_autonomy/issues/18) + Various
comments for enhancements
- [Rachel Brindle](https://github.com/younata) - [Enhanced Navdata for AR-Drone 2.0](https://github.com/AutonomyLab/ardrone_autonomy/pull/2)
## FAQ
src/ardrone_driver.cpp
+71 -52
Ver Arquivo
@@ -14,17 +14,18 @@ ARDroneDriver::ARDroneDriver()
last_frame_id = -1;
last_navdata_id = -1;
cmd_vel_sub = node_handle.subscribe("cmd_vel", 1, &cmdVelCallback);
takeoff_sub = node_handle.subscribe("ardrone/takeoff", 1, &takeoffCallback);
reset_sub = node_handle.subscribe("ardrone/reset", 1, &resetCallback);
land_sub = node_handle.subscribe("ardrone/land", 1, &landCallback);
image_pub = image_transport.advertiseCamera("ardrone/image_raw", 10);
takeoff_sub = node_handle.subscribe("ardrone/takeoff", 1, &takeoffCallback);
reset_sub = node_handle.subscribe("ardrone/reset", 1, &resetCallback);
land_sub = node_handle.subscribe("ardrone/land", 1, &landCallback);
image_pub = image_transport.advertiseCamera("ardrone/image_raw", 10);
hori_pub = image_transport.advertiseCamera("ardrone/front/image_raw", 10);
vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 10);
vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 10);
navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
flatTrim_service = node_handle.advertiseService("ardrone/flattrim", flatTrimCallback);
/*
@@ -34,8 +35,8 @@ ARDroneDriver::ARDroneDriver()
imuReCalib_service = node_handle.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>
("ardrone/imu_recalib", boost::bind(&ARDroneDriver::imuReCalibCallback, this, _1, _2));
// setEnemyColor_service = node_handle.advertiseService("/ardrone/setenemycolor", setEnemyColorCallback);
// setHullType_service = node_handle.advertiseService("/ardrone/sethulltype", setHullTypeCallback);
// setEnemyColor_service = node_handle.advertiseService("/ardrone/setenemycolor", setEnemyColorCallback);
// setHullType_service = node_handle.advertiseService("/ardrone/sethulltype", setHullTypeCallback);
droneFrameId = (ros::param::get("~drone_frame_id", droneFrameId)) ? droneFrameId : "ardrone_base";
droneFrameBase = droneFrameId + "_link";
@@ -104,7 +105,7 @@ ARDroneDriver::ARDroneDriver()
{
tf_base_bottom.setData(tf_base_bottom.inverse());
tf_base_bottom.child_frame_id_.swap(tf_base_bottom.frame_id_);
}
}
}
@@ -613,29 +614,31 @@ void ARDroneDriver::publish_navdata()
navdata.vz -= vel_bias[2];
}
if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0))
if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0))
return; // why bother, no one is listening.
ardrone_autonomy::Navdata msg;
const ros::Time _now = ros::Time::now();
msg.header.stamp = ros::Time::now();
ardrone_autonomy::Navdata msg;
msg.header.stamp = _now;
msg.header.frame_id = droneFrameBase;
msg.batteryPercent = navdata.vbat_flying_percentage;
msg.batteryPercent = navdata.vbat_flying_percentage;
msg.state = (navdata.ctrl_state >> 16);
// positive means counterclockwise rotation around axis
msg.rotX = navdata.phi / 1000.0; // tilt left/right
msg.rotY = -navdata.theta / 1000.0; // tilt forward/backward
msg.rotZ = -navdata.psi / 1000.0; // orientation
// positive means counterclockwise rotation around axis
msg.rotX = navdata.phi / 1000.0; // tilt left/right
msg.rotY = -navdata.theta / 1000.0; // tilt forward/backward
msg.rotZ = -navdata.psi / 1000.0; // orientation
msg.altd = navdata.altitude; // cm
msg.vx = navdata.vx; // mm/sec
msg.vy = -navdata.vy; // mm/sec
msg.vz = -navdata.vz; // mm/sec
msg.tm = arnavtime.time;
msg.ax = navdata_phys.phys_accs[ACC_X] / 1000.0; // g
msg.ay = -navdata_phys.phys_accs[ACC_Y] / 1000.0; // g
msg.az = -navdata_phys.phys_accs[ACC_Z] / 1000.0; // g
msg.altd = navdata.altitude; // cm
msg.vx = navdata.vx; // mm/sec
msg.vy = -navdata.vy; // mm/sec
msg.vz = -navdata.vz; // mm/sec
msg.tm = arnavtime.time;
msg.ax = navdata_phys.phys_accs[ACC_X] / 1000.0; // g
msg.ay = -navdata_phys.phys_accs[ACC_Y] / 1000.0; // g
msg.az = -navdata_phys.phys_accs[ACC_Z] / 1000.0; // g
// New stuff
if (IS_ARDRONE2)
@@ -661,35 +664,33 @@ void ARDroneDriver::publish_navdata()
msg.wind_comp_angle = 0.0;
}
// Tag Detection
msg.tags_count = navdata_detect.nb_detected;
// Tag Detection
msg.tags_count = navdata_detect.nb_detected;
for (int i = 0; i < navdata_detect.nb_detected; i++)
{
/*
* The tags_type is in raw format. In order to extract the information
* macros from ardrone_api.h is needed.
*
* #define DETECTION_MAKE_TYPE(source,tag) ( ((source)<<16) | (tag) )
* #define DETECTION_EXTRACT_SOURCE(type) ( ((type)>>16) & 0x0FF )
* #define DETECTION_EXTRACT_TAG(type) ( (type) & 0x0FF )
*
* Please also note that the xc, yc, width and height are in [0,1000] range
* and must get converted back based on image resolution.
*/
msg.tags_type.push_back(navdata_detect.type[i]);
msg.tags_xc.push_back(navdata_detect.xc[i]);
msg.tags_yc.push_back(navdata_detect.yc[i]);
msg.tags_width.push_back(navdata_detect.width[i]);
msg.tags_height.push_back(navdata_detect.height[i]);
msg.tags_orientation.push_back(navdata_detect.orientation_angle[i]);
msg.tags_distance.push_back(navdata_detect.dist[i]);
}
navdata_pub.publish(msg);
{
/*
* The tags_type is in raw format. In order to extract the information
* macros from ardrone_api.h is needed.
*
* #define DETECTION_MAKE_TYPE(source,tag) ( ((source)<<16) | (tag) )
* #define DETECTION_EXTRACT_SOURCE(type) ( ((type)>>16) & 0x0FF )
* #define DETECTION_EXTRACT_TAG(type) ( (type) & 0x0FF )
*
* Please also note that the xc, yc, width and height are in [0,1000] range
* and must get converted back based on image resolution.
*/
msg.tags_type.push_back(navdata_detect.type[i]);
msg.tags_xc.push_back(navdata_detect.xc[i]);
msg.tags_yc.push_back(navdata_detect.yc[i]);
msg.tags_width.push_back(navdata_detect.width[i]);
msg.tags_height.push_back(navdata_detect.height[i]);
msg.tags_orientation.push_back(navdata_detect.orientation_angle[i]);
msg.tags_distance.push_back(navdata_detect.dist[i]);
}
/* IMU */
imu_msg.header.frame_id = droneFrameBase;
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.stamp = _now;
// IMU - Linear Acc
imu_msg.linear_acceleration.x = msg.ax * 9.8;
@@ -707,6 +708,24 @@ void ARDroneDriver::publish_navdata()
imu_msg.angular_velocity.y = -navdata_phys.phys_gyros[GYRO_Y] * DEG_TO_RAD;
imu_msg.angular_velocity.z = -navdata_phys.phys_gyros[GYRO_Z] * DEG_TO_RAD;
mag_msg.header.frame_id = droneFrameBase;
mag_msg.header.stamp = _now;
const float mag_normalizer = sqrt( msg.magX * msg.magX + msg.magY * msg.magY + msg.magZ * msg.magZ );
// TODO: Check if it is really needed that magnetometer message includes normalized value
if (fabs(mag_normalizer) > 1e-9f)
{
mag_msg.vector.x = msg.magX / mag_normalizer;
mag_msg.vector.y = msg.magY / mag_normalizer;
mag_msg.vector.z = msg.magZ / mag_normalizer;
mag_pub.publish(mag_msg);
}
else
{
ROS_WARN_THROTTLE(1, "There is something wrong with the magnetometer readings (Magnitude is extremely small).");
}
navdata_pub.publish(msg);
imu_pub.publish(imu_msg);
}
src/ardrone_driver.h
+3
Ver Arquivo
@@ -7,6 +7,7 @@
#include <camera_info_manager/camera_info_manager.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <std_srvs/Empty.h>
#include <ardrone_autonomy/Navdata.h>
#include "ardrone_sdk.h"
@@ -64,6 +65,7 @@ private:
ros::Publisher navdata_pub;
ros::Publisher imu_pub;
ros::Publisher mag_pub;
tf::TransformBroadcaster tf_broad;
@@ -113,6 +115,7 @@ private:
// Huge part of IMU message is constant, let's fill'em once.
sensor_msgs::Imu imu_msg;
geometry_msgs::Vector3Stamped mag_msg;
// Manual IMU caliberation
bool do_caliberation;