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)
{
ROS_WARN("%s", ex.what());
ros::Duration(0.01).sleep();
//return;
}
}
return map_point_output;
}
move_base_msgs::MoveBaseGoal buildGoal(geometry_msgs::PointStamped map_point, geometry_msgs::PointStamped map_next, bool last_point)
{
move_base_msgs::MoveBaseGoal goal;
//Specify what frame we want the goal to be published in
goal.target_pose.header.frame_id = "odom";
goal.target_pose.header.stamp = ros::Time::now();
// Specify x and y goal
goal.target_pose.pose.position.x = map_point.point.x; //specify x goal
goal.target_pose.pose.position.y = map_point.point.y; //specify y goal
// Specify heading goal using current goal and next goal (point robot towards its next goal once it has achieved its current goal)
if(last_point == false)
{
tf::Matrix3x3 rot_euler;
tf::Quaternion rot_quat;
// Calculate quaternion
float x_curr = map_point.point.x, y_curr = map_point.point.y; // set current coords.
float x_next = map_next.point.x, y_next = map_next.point.y; // set coords. of next waypoint
float delta_x = x_next - x_curr, delta_y = y_next - y_curr; // change in coords.
float yaw_curr = 0, pitch_curr = 0, roll_curr = 0;
yaw_curr = atan2(delta_y, delta_x);
// Specify quaternions
rot_euler.setEulerYPR(yaw_curr, pitch_curr, roll_curr);
rot_euler.getRotation(rot_quat);
goal.target_pose.pose.orientation.x = rot_quat.getX();
goal.target_pose.pose.orientation.y = rot_quat.getY();
goal.target_pose.pose.orientation.z = rot_quat.getZ();
goal.target_pose.pose.orientation.w = rot_quat.getW();
}
else
{
goal.target_pose.pose.orientation.w = 1.0;
}
return goal;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "gps_waypoint"); //initiate node called gps_waypoint
ros::NodeHandle n;
ROS_INFO("Initiated gps_waypoint node");
MoveBaseClient ac("/move_base", true);
//construct an action client that we use to communication with the action named move_base.
//Setting true is telling the constructor to start ros::spin()
// Initiate publisher to send end of node message
ros::Publisher pubWaypointNodeEnded = n.advertise<std_msgs::Bool>("/outdoor_waypoint_nav/waypoint_following_status", 100);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0)))
{
wait_count++;
if(wait_count > 3)
{
ROS_ERROR("move_base action server did not come up, killing gps_waypoint node...");
// Notify joy_launch_control that waypoint following is complete
std_msgs::Bool node_ended;
node_ended.data = true;
pubWaypointNodeEnded.publish(node_ended);
ros::shutdown();
}
ROS_INFO("Waiting for the move_base action server to come up");
}
//Get Longitude and Latitude goals from text file
//Count number of waypoints
ros::param::get("/outdoor_waypoint_nav/coordinates_file", path_local);
numWaypoints = countWaypointsInFile(path_local);
//Reading waypoints from text file and output results
waypointVect = getWaypoints(path_local);
// Iterate through vector of waypoints for setting goals
for(iter = waypointVect.begin(); iter < waypointVect.end(); iter++)
{
//Setting goal:
latiGoal = iter->first;
longiGoal = iter->second;
bool final_point = false;
//set next goal point if not at last waypoint
if(iter < (waypointVect.end() - 1))
{
iter++;
latiNext = iter->first;
longiNext = iter->second;
iter--;
}
else //set to current
{
latiNext = iter->first;
longiNext = iter->second;
final_point = true;
}
ROS_INFO("Received Latitude goal:%.8f", latiGoal);
ROS_INFO("Received longitude goal:%.8f", longiGoal);
//Convert lat/long to utm:
UTM_point = latLongtoUTM(latiGoal, longiGoal);
UTM_next = latLongtoUTM(latiNext, longiNext);
//Transform UTM to map point in odom frame
map_point = UTMtoMapPoint(UTM_point);
map_next = UTMtoMapPoint(UTM_next);
//Build goal to send to move_base
move_base_msgs::MoveBaseGoal goal = buildGoal(map_point, map_next, final_point); //initiate a move_base_msg called goal
// Send Goal
ROS_INFO("Sending goal");
ac.sendGoal(goal); //push goal to move_base node
//Wait for result
ac.waitForResult(); //waiting to see if move_base was able to reach goal
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("Husky has reached its goal!");
//switch to next waypoint and repeat
}
else
{
ROS_ERROR("Husky was unable to reach its goal. GPS Waypoint unreachable.");
ROS_INFO("Exiting node...");
// Notify joy_launch_control that waypoint following is complete
std_msgs::Bool node_ended;
node_ended.data = true;
pubWaypointNodeEnded.publish(node_ended);
ros::shutdown();
}
Py_Initialize();
FILE*PythonScriptFile;
if(PythonScriptFile == NULL)
{
printf(".py is already open\n");
PySys_SetArgv( argc, argv);
PyRun_SimpleFile( PythonScriptFile, "/.../scripts/take_photo.py");
fclose(PythonScriptFile);
printf(".py closed\n");
}
else
{
printf(".py is opening\n");
PythonScriptFile = fopen("/.../scripts/take_photo.py", "r");
printf("First .py opened\n");
PySys_SetArgv( argc, argv);
PyRun_SimpleFile( PythonScriptFile, "/.../scripts/take_photo.py");
fclose(PythonScriptFile);
printf("First .py closed\n");
}
Py_Finalize();
}
// End for loop iterating through waypoint vector
ROS_INFO("Husky has reached all of its goals!!!\n");
ROS_INFO("Ending node...");
// Notify joy_launch_control that waypoint following is complete
std_msgs::Bool node_ended;
node_ended.data = true;
pubWaypointNodeEnded.publish(node_ended);
ros::shutdown();
ros::spin();
return 0;
}
My script to take a picture is in .py format. So I'm trying to call a Python script from within C++. For this, the lines I added to the gps_waypoint.cpp file are as follows.
Py_Initialize();
FILE*PythonScriptFile;
if(PythonScriptFile == NULL)
{
printf(".py is already open \n");
PySys_SetArgv( argc, argv);
PyRun_SimpleFile( PythonScriptFile, "/.../scripts/take_photo.py");
fclose(PythonScriptFile);
printf(".py closed\n");
}
else
{
printf(".py is opening\n");
PythonScriptFile = fopen("/.../scripts/take_photo.py", "r");
printf("First .py opened\n");
PySys_SetArgv( argc, argv);
PyRun_SimpleFile( PythonScriptFile, "/.../scripts/take_photo.py");
fclose(PythonScriptFile);
printf("First .py closed\n");
}
Py_Finalize();
When I run the package, the script works properly for the first GPS point and takes the photo. However, at the 2nd GPS point, the program suddenly fails.
SUMMARY
========
PARAMETERS
* /outdoor_waypoint_nav/coordinates_file: /waypoint_files/w...
* /rosdistro: melodic
* /rosversion: 1.14.12
NODES
/outdoor_waypoint_nav/
gps_waypoint (outdoor_waypoint_nav/gps_waypoint)
ROS_MASTER_URI=http://localhost:11311
process[outdoor_waypoint_nav/gps_waypoint-1]: started with pid [17452]
[ INFO] [1644936589.889007182]: Initiated gps_waypoint node
[ INFO] [1644936590.417971839, 16.209000000]: 4 GPS waypoints were read
[ INFO] [1644936590.438660362, 16.230000000]: The following GPS Waypoints have been set:
[ INFO] [1644936590.438690200, 16.230000000]: 50.62492 6.98784
[ INFO] [1644936590.438702833, 16.230000000]: 50.624925 6.98784
[ INFO] [1644936590.438711339, 16.230000000]: 50.62493 6.98784
[ INFO] [1644936590.438720511, 16.230000000]: 50.624935 6.98784
[ INFO] [1644936590.438733668, 16.230000000]: Received Latitude goal:50.62492000
[ INFO] [1644936590.438746266, 16.230000000]: Received longitude goal:6.98784000
[ INFO] [1644936596.474889006, 22.255000000]: Sending goal
[ INFO] [1644936614.003555129, 39.755000000]: Husky has reached its goal!
.py is opening
First .py opened
[INFO] [1644936618.652097, 44.395000]: Image received.
First .py closed
[ INFO] [1644936620.798132564, 46.538000000]: Received Latitude goal:50.62492500
[ INFO] [1644936620.798173290, 46.538000000]: Received longitude goal:6.98784000
[ INFO] [1644936626.844166665, 52.569000000]: Sending goal
[ INFO] [1644936633.858664942, 59.569000000]: Husky has reached its goal!
[outdoor_waypoint_nav/gps_waypoint-1] process has died [pid 17452, exit code -11, cmd /.../catkin_ws/devel/lib/outdoor_waypoint_nav/gps_waypoint __name:=gps_waypoint __log:=/.../.ros/log/7b049686-8e6e-11ec-bb9e-747827df7356/outdoor_waypoint_nav-gps_waypoint-1.log].
log file: /.../.ros/log/7b049686-8e6e-11ec-bb9e-747827df7356/outdoor_waypoint_nav-gps_waypoint-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done
What could be the reason for that? Probably, there is a mistake while calling the python file multiple times in C++(maybe opening a file that is already open), but I couldn't figure it out. Could you help me with that?
Asked by outdoorr_robotics on 2022-02-15 10:31:19 UTC
Comments
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/WritingPublisherSubscriber%28python%29
Asked by osilva on 2022-02-16 08:01:45 UTC
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.
Asked by outdoorr_robotics on 2022-02-21 07:38:42 UTC