Hello,
I want to draw a square with a rover, using Ardupilot. For that, I use setpoint_position/local. My script is here, but let me summarize it: I take the coordinate data from GPS, then I calculate the distance between the waypoint and target point, if the distance is less than 1 m, I switch to the next waypoint. I publish the setpoint message around 10 Hz (using ros rate(10)). My script does not work, because the rover either never moves, or moves for a short distances and then stops.
In my script, I use GUIDED mode, and I specify x, y, and z coordinates. I never set a stamp for any message. Also, I don’t use OFFBOARD mode. In SITL, my code works without problem, in real rover it doesn’t work. Also, if I publish a point via rostopic pub setpoint_position, it works on my rover. Here is my code:
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <vector>
#include <tuple>
geometry_msgs::PoseStamped pointToGo;
double posX = 0;
double posY = 0;
double posZ = 0;
double orientation = 0;
double direction = 0;
std::vector<std::tuple<double, double, double>> waypoints;
double targetX = 0;
double targetY = 0;
double targetZ = 0;
int counter = 0;
double targetDistance = 10;
double targetReached = 1;
bool over = false;
void pos_callback(const nav_msgs::Odometry::ConstPtr& msg){
const nav_msgs::Odometry current_pos = *msg;
const double posX_before = posX;
const double posY_before = posY;
posX = current_pos.pose.pose.position.x;
posY = current_pos.pose.pose.position.y;
posZ = current_pos.pose.pose.position.z;
direction = -atan2(posX-posX_before, posY-posY_before);
targetDistance = sqrt((posX-targetX)*(posX-targetX)+(posY-targetY)*(posY-targetY));
geometry_msgs::Quaternion q = current_pos.pose.pose.orientation;
orientation = -atan2(1-2*(q.y*q.y + q.z*q.z), 2*(q.w*q.z + q.x*q.y));
std::cout << "target x: " << targetX << " target y: " << targetY << " dist: " << targetDistance << std::endl;
std::cout << "Direction " << direction << " Orientation " << orientation << std::endl;
if (targetDistance < targetReached){
counter++;
ROS_INFO("Waypoint reached!");
if(counter == waypoints.size()){
over = true;
ROS_INFO("Shutting down!");
}
}
}
int main(int argc, char** argv){
ros::init(argc, argv, "waypoint_node");
ros::NodeHandle nh;
ros::Subscriber pos_sub = nh.subscribe<nav_msgs::Odometry> ("/mavros/global_position/local", 1, pos_callback);
ros::Publisher pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/local", 1);
ros::Rate rate(10);
waypoints.emplace_back(0,5,2);
waypoints.emplace_back(5,5,2);
waypoints.emplace_back(5,0,2);
waypoints.emplace_back(0,0,2);
while(ros::ok()){
targetX = std::get<0>(waypoints[counter]);
targetY = std::get<1>(waypoints[counter]);
targetZ = std::get<2>(waypoints[counter]);
pointToGo.pose.position.x = targetX;
pointToGo.pose.position.y = targetY;
pointToGo.pose.position.z = targetZ;
if (over){
pointToGo.pose.position.x = 0;
pointToGo.pose.position.y = 0;
pointToGo.pose.position.z = 0;
}
pos_pub.publish(pointToGo);
ros::spin();
rate.sleep();
if (over){
//ros::shutdown()
break;
}
}
}
Thank you.