void
poseStampedCallback(const geometry_msgs::PoseStamped& poseStampedMsg)
{
// set timestamp (get NSec from ROS and convert to us)
uint64_t timestamp = poseStampedMsg.header.stamp.toNSec() / 1000;
// send MAVLINK attitude and local position messages
mavlink_message_t msg;
//convert quaternion to euler angles
const btQuaternion quat(poseStampedMsg.pose.orientation.x,
poseStampedMsg.pose.orientation.y,
poseStampedMsg.pose.orientation.z,
poseStampedMsg.pose.orientation.w);
const btMatrix3x3 mat(quat);
double roll, pitch, yaw;
mat.getEulerYPR(yaw, pitch, roll);
mavlink_msg_attitude_pack(sysid, compid, &msg, timestamp, roll, pitch, yaw, 0.0f, 0.0f, 0.0f);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
float x = poseStampedMsg.pose.position.x;
float y = poseStampedMsg.pose.position.y;
float z = poseStampedMsg.pose.position.z;
mavlink_msg_local_position_pack(sysid, compid, &msg, timestamp, x, y, z, 0.0f, 0.0f, 0.0f);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
if (verbose)
{
ROS_INFO("Sent Mavlink local position and attitude messages.");
}
}
/*
* @brief emits a message that a waypoint reached
*
* This function broadcasts a message that a waypoint is reached.
*
* @param seq The waypoint sequence number the MAV has reached.
*/
void mavlink_wpm_send_waypoint_reached(uint16_t seq)
{
mavlink_message_t msg;
mavlink_waypoint_reached_t wp_reached;
wp_reached.seq = seq;
mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq);
usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
void mavlink_wpm_send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count)
{
mavlink_message_t msg;
mavlink_waypoint_count_t wpc;
wpc.target_system = target_systemid;
wpc.target_component = target_compid;
wpc.count = count;
mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
/*
* @brief Sends an waypoint ack message
*/
void mavlink_wpm_send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type)
{
mavlink_message_t msg;
mavlink_waypoint_ack_t wpa;
wpa.target_system = target_systemid;
wpa.target_component = target_compid;
wpa.type = type;
mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
usleep(paramClient->getParamValue("PROTOCOLDELAY"));
if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
}
void mavlink_wpm_send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq)
{
if (seq < waypoints->size())
{
mavlink_message_t msg;
mavlink_waypoint_t *wp = waypoints->at(seq);
wp->target_system = target_systemid;
wp->target_component = target_compid;
mavlink_msg_waypoint_encode(systemid, compid, &msg, wp);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
else
{
if (verbose) printf("ERROR: index out of bounds\n");
}
}
void
paramCheckCallback(const ros::TimerEvent&)
{
bool homeShift = false;
double latitude;
if (nh->getParamCached("/gps_ref_latitude", latitude) &&
latitude != homeLatitude)
{
homeLatitude = latitude;
homeShift = true;
}
double longitude;
if (nh->getParamCached("/gps_ref_longitude", longitude) &&
longitude != homeLongitude)
{
homeLongitude = longitude;
homeShift = true;
}
double altitude;
if (nh->getParamCached("/gps_ref_altitude", altitude) &&
altitude != homeAltitude)
{
homeAltitude = altitude;
homeShift = true;
}
if (homeShift)
{
mavlink_message_t msg;
mavlink_msg_gps_local_origin_set_pack(sysid, compid, &msg,
homeLatitude, homeLongitude, homeAltitude);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
if (verbose)
{
ROS_INFO("Sent Mavlink GPS local origin set message.");
}
}
}
/*
* @brief Directs the MAV to fly to a position
*
* Sends a message to the controller, advising it to fly to the coordinates
* of the waypoint with a given orientation
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void mavlink_wpm_send_setpoint(uint16_t seq)
{
if(seq < waypoints->size())
{
mavlink_waypoint_t *cur = waypoints->at(seq);
mavlink_message_t msg;
mavlink_local_position_setpoint_set_t PControlSetPoint;
// send new set point to local IMU
if (cur->frame == 1)
{
PControlSetPoint.target_system = systemid;
PControlSetPoint.target_component = MAV_COMP_ID_IMU;
PControlSetPoint.x = cur->x;
PControlSetPoint.y = cur->y;
PControlSetPoint.z = cur->z;
PControlSetPoint.yaw = cur->param4;
mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
else
{
if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
}
struct timeval tv;
gettimeofday(&tv, NULL);
uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
timestamp_last_send_setpoint = now;
}
else
{
if (verbose) printf("ERROR: index out of bounds\n");
}
}
/*
* @brief Broadcasts the new target waypoint and directs the MAV to fly there
*
* This function broadcasts its new active waypoint sequence number and
* sends a message to the controller, advising it to fly to the coordinates
* of the waypoint with a given orientation
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void mavlink_wpm_send_waypoint_current(uint16_t seq)
{
if(seq < waypoints->size())
{
mavlink_waypoint_t *cur = waypoints->at(seq);
mavlink_message_t msg;
mavlink_waypoint_current_t wpc;
wpc.seq = cur->seq;
mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc);
mavlink_message_t_publish(lcm, "MAVLINK", &msg);
usleep(paramClient->getParamValue("PROTOCOLDELAY"));
if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq);
}
else
{
if (verbose) printf("ERROR: index out of bounds\n");
}
}
