Error while calling Python script multiple times in a C++ loop
Hello,
I want to integrate a camera into autonomous GPS waypoint navigation. The main goal is when the robot reaches the GPS waypoint, it should turn on the camera to take a picture and turn the camera off after it took a picture and do this for all GPS waypoints. I'm using ROS Melodic on Ubuntu 18.04.6 LTS.
The ROS package I used for the GPS waypoint is here. I am trying to modify the gps_waypoint.cpp file in this package. My goal is to add some lines in that file to run the script that allows me to take a photo when the robot succeeded in reaching the GPS waypoint. The modified .cpp file is as follows.
#include <ros/ros.h>
#include <ros/package.h>
#include <fstream>
#include <utility>
#include <vector>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <robot_localization/navsat_conversions.h>
#include <geometry_msgs/PointStamped.h>
#include <std_msgs/Bool.h>
#include <tf/transform_listener.h>
#include <math.h>
#include <iostream>
#include </usr/include/python2.7/Python.h>
#include <stdio.h>
#include <conio.h>
// initialize variables
typedef actionlib::SimpleActionClient <move_base_msgs::MoveBaseAction>
MoveBaseClient; //create a type definition for a client called MoveBaseClient
std::vector <std::pair<double, double>> waypointVect;
std::vector<std::pair < double, double> > ::iterator iter; //init. iterator
geometry_msgs::PointStamped UTM_point, map_point, UTM_next, map_next;
int count = 0, waypointCount = 0, wait_count = 0;
double numWaypoints = 0;
double latiGoal, longiGoal, latiNext, longiNext;
std::string utm_zone;
std::string path_local, path_abs;
int countWaypointsInFile(std::string path_local)
{
path_abs = ros::package::getPath("outdoor_waypoint_nav") + path_local;
std::ifstream fileCount(path_abs.c_str());
if(fileCount.is_open())
{
double lati = 0;
while(!fileCount.eof())
{
fileCount >> lati;
++count;
}
count = count - 1;
numWaypoints = count / 2;
ROS_INFO("%.0f GPS waypoints were read", numWaypoints);
fileCount.close();
}
else
{
std::cout << "Unable to open waypoint file" << std::endl;
ROS_ERROR("Unable to open waypoint file");
}
return numWaypoints;
}
std::vector <std::pair<double, double>> getWaypoints(std::string path_local)
{
double lati = 0, longi = 0;
path_abs = ros::package::getPath("outdoor_waypoint_nav") + path_local;
std::ifstream fileRead(path_abs.c_str());
for(int i = 0; i < numWaypoints; i++)
{
fileRead >> lati;
fileRead >> longi;
waypointVect.push_back(std::make_pair(lati, longi));
}
fileRead.close();
//Outputting vector
ROS_INFO("The following GPS Waypoints have been set:");
for(std::vector < std::pair < double, double >> ::iterator iterDisp = waypointVect.begin(); iterDisp != waypointVect.end();
iterDisp++)
{
ROS_INFO("%.9g %.9g", iterDisp->first, iterDisp->second);
}
return waypointVect;
}
geometry_msgs::PointStamped latLongtoUTM(double lati_input, double longi_input)
{
double utm_x = 0, utm_y = 0;
geometry_msgs::PointStamped UTM_point_output;
//convert lat/long to utm
RobotLocalization::NavsatConversions::LLtoUTM(lati_input, longi_input, utm_y, utm_x, utm_zone);
//Construct UTM_point and map_point geometry messages
UTM_point_output.header.frame_id = "utm";
UTM_point_output.header.stamp = ros::Time(0);
UTM_point_output.point.x = utm_x;
UTM_point_output.point.y = utm_y;
UTM_point_output.point.z = 0;
return UTM_point_output;
}
geometry_msgs::PointStamped UTMtoMapPoint(geometry_msgs::PointStamped UTM_input)
{
geometry_msgs::PointStamped map_point_output;
bool notDone = true;
tf::TransformListener listener; //create transformlistener object called listener
ros::Time time_now = ros::Time::now();
while(notDone)
{
try
{
UTM_point.header.stamp = ros::Time::now();
listener.waitForTransform("odom", "utm", time_now, ros::Duration(3.0));
listener.transformPoint("odom", UTM_input, map_point_output);
notDone = false;
}
catch (tf::TransformException& ex ...
Why don't you create a Python ROS node that subscribes to your C++ node (waypoints) instead of calling by opening python file.
Refer to this tutorial: http://wiki.ros.org/ROS/Tutorials/Wri...
Thanks! As you said, I created a python node that includes my take_photo node and I subscribed to it in my waypoint node instead of calling the script every time. It is working now. Thank you.