Hello everyone.
How can I send home position from my fake gps module during initialization phase?
I’m currently working in SITL mode in gazebo, and I have removed the gps joint from iris model so far so good.
I also wrote a simple ros node that keeps generating simulated gps coordinate which is like this :
#include <limits>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <sensor_msgs/NavSatFix.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <mavros_msgs/Altitude.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#define ALT 2 // fly altitude in meters
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr &msg)
{
current_state = *msg;
}
double latitude, longitude;
bool gps_received = false;
void get_global_pose(const sensor_msgs::NavSatFix::ConstPtr &msg)
{
// vertical coord like y
latitude = msg->latitude;
// horizontal coord like x
longitude = msg->longitude;
gps_received = true;
ROS_INFO("latitue: %.6f longitude: %.6f altitude: %.4f", latitude, longitude, msg->altitude);
}
double altitude;
void get_altitude(const mavros_msgs::Altitude::ConstPtr &msg)
{
altitude = msg->amsl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "offb_node_gps_puber");
ros::NodeHandle nh;
// set gps
// setpoint_position/global
ros::Publisher pub_global_gps = nh.advertise<sensor_msgs::NavSatFix>("mavros/global_position/global", 10);
sensor_msgs::NavSatFix gps_target;
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for GPS connection
int i = 0;
gps_target.longitude = 20;
while (ros::ok())
{
gps_target.header.stamp = ros::Time::now();
gps_target.header.frame_id = "map";
gps_target.header.seq = i++;
//https://answers.ros.org/question/10310/calculate-navsatfix-covariance/
gps_target.position_covariance_type = gps_target.COVARIANCE_TYPE_DIAGONAL_KNOWN;
//sample covariance taken from fake gps. I either should use Unknown, or
//COVARIANCE_TYPE_APPROXIMATED and come up with a proper covariance, atm I have no idea!
gps_target.position_covariance = {1, 0, 0, 0, 1, 0, 0, 0, 1};
//status :https://wiki.ros.org/sensor_msgs/Reviews/2010-05-31%20GPS%20Proposal_API_Review
//https://answers.ros.org/question/319109/meaning-of-status-fields-in-sensor_msgsnavsatstatus/
//http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatStatus.html
/*
# Navigation Satellite fix status for any Global Navigation Satellite System
# Whether to output an augmented fix is determined by both the fix
# type and the last time differential corrections were received. A
# fix is valid when status >= STATUS_FIX.
int8 STATUS_NO_FIX = -1 # unable to fix position
int8 STATUS_FIX = 0 # unaugmented fix
int8 STATUS_SBAS_FIX = 1 # with satellite-based augmentation
int8 STATUS_GBAS_FIX = 2 # with ground-based augmentation
int8 status
# Bits defining which Global Navigation Satellite System signals were
# used by the receiver.
uint16 SERVICE_GPS = 1
uint16 SERVICE_GLONASS = 2
uint16 SERVICE_COMPASS = 4 # includes BeiDou.
uint16 SERVICE_GALILEO = 8
uint16 service
*/
gps_target.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
gps_target.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
//random location
gps_target.latitude = 20;
gps_target.longitude = 20;
//what about the height? should we use the local height?
//should we go to 2m and then set this to 2? what if we decline?
//should we use a specific altometer ?
gps_target.altitude = ALT;
pub_global_gps.publish(gps_target);
ros::spinOnce();
rate.sleep();
}
return 0;
}
The problem is, when I start the simulation, at the initialization phase, it gets stuck trying to get home position:
[ INFO] [1647069881.031994618]: HP: requesting home position
[ INFO] [1647069891.031633725]: HP: requesting home position
[ERROR] [1647069891.489842169]: TM : Time jump detected. Resetting time synchroniser.
[ INFO] [1647069901.030425796]: HP: requesting home position
[ INFO] [1647069911.036741222]: HP: requesting home position
[ INFO] [1647069921.030996704]: HP: requesting home position
[ INFO] [1647069931.030818858]: HP: requesting home position
[ INFO] [1647069941.030857069]: HP: requesting home position
[ERROR] [1647069945.789897360]: TM : Time jump detected. Resetting time synchroniser.
[ INFO] [1647069951.030237748]: HP: requesting home position
[ INFO] [1647069961.030462248]: HP: requesting home position
[ INFO] [1647069971.030516927]: HP: requesting home position
[ INFO] [1647069981.030705622]: HP: requesting home position
[ INFO] [1647069991.030550798]: HP: requesting home position
[ INFO] [1647070001.043023776]: HP: requesting home position
[ INFO] [1647070011.043772037]: HP: requesting home position
[ERROR] [1647070019.989296071]: TM : Time jump detected. Resetting time synchroniser.
[ INFO] [1647070021.030486418]: HP: requesting home position
[ INFO] [1647070031.030343993]: HP: requesting home position
What should I do to fix this issue?
Side note:
As to why I’m doing this, what I’m trying to do, is to calculate GPS coordinate and instead of physical gps module, do it in software mode.
Thanks a lot in advance.