Error while calling Python script multiple times in a C++ loop

asked 2022-02-15 09:31:19 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

1

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...

osilva gravatar image osilva  ( 2022-02-16 07:01:45 -0500 )edit

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.

outdoorr_robotics gravatar image outdoorr_robotics  ( 2022-02-21 06:38:42 -0500 )edit