Combining two nodes using a launch-file , and cv2.imwrite() not working as a result

Question

(I am using ros melodic, on ubuntu 18.04, and turtlebot-2)

I have two python files goforward.py: which keeps moving the turtlebot forward untill we terminate using ctrl+C, and take_photo_mod.py, which uses the camera to take photos at 1 second intervals, and saves them to the cwd (and keeps running until we terminate using ctrl+C).

goforward.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist

class GoForward():
    def __init__(self):
        # initiliaze
        rospy.init_node('GoForward', anonymous=False)

    # tell user how to stop TurtleBot
    rospy.loginfo("To stop TurtleBot CTRL + C")

        # What function to call when you ctrl + c    
        rospy.on_shutdown(self.shutdown)
        
    # Create a publisher which can "talk" to TurtleBot and tell it to move
        # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2
        self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
     
    #TurtleBot will stop if we don't keep telling it to move.  How often should we tell it to move? 10 HZ
        r = rospy.Rate(10);

        # Twist is a datatype for velocity
        move_cmd = Twist()
    # let's go forward at 0.2 m/s
        move_cmd.linear.x = 0.2
    # let's turn at 0 radians/s
    move_cmd.angular.z = 0

    # as long as you haven't ctrl + c keeping doing...
        while not rospy.is_shutdown():
        # publish the velocity
            self.cmd_vel.publish(move_cmd)
        # wait for 0.1 seconds (10 HZ) and publish again
            r.sleep()
                        
        
    def shutdown(self):
        # stop turtlebot
        rospy.loginfo("Stop TurtleBot")
    # a default Twist has linear.x of 0 and angular.z of 0.  So it'll stop TurtleBot
        self.cmd_vel.publish(Twist())
    # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        GoForward()
    except:
        rospy.loginfo("GoForward node terminated.")

take_photo_mod.py

#!/usr/bin/env python
from __future__ import print_function
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class TakePhoto:
    def __init__(self):

        self.bridge = CvBridge()
        self.image_received = False

        # Connect image topic
        img_topic = "/camera/rgb/image_raw"
        self.image_sub = rospy.Subscriber(img_topic, Image, self.callback)

        # Allow up to one second to connection
        rospy.sleep(1)

    def callback(self, data):

        # Convert image to OpenCV format
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

        self.image_received = True
        self.image = cv_image

    def take_picture(self, img_title):
        if self.image_received:
            # Save an image
            cv2.imwrite(img_title, self.image)
            rospy.loginfo("received")

            return True
        else:
            return False

if __name__ == '__main__':

    # Initialize
    rospy.init_node('take_photo', anonymous=False)
    camera = TakePhoto()

    # Take a photo

    # Use '_image_title' parameter from command line
    # Default value is 'photo.jpg'
    while not rospy.is_shutdown():
        
        img_title = rospy.get_param('~image_title', 'photo1.jpg')

        if camera.take_picture(img_title):
            rospy.loginfo("Saved image " + img_title)
        else:
            rospy.loginfo("No images received")

        # Sleep to give the last log messages time to be sent
        rospy.sleep(1)

These two programs individually, are running flawlessly. I can see the turtlebot moving in the environment. I can also see that the images taken are saved to the working directory. Now, I wanted to use these nodes simultaneously, and the way to do so is to create launch files.

So I created a launchfile that had these two nodes in it:

Upon launching the launch file, I get no errors. The terminal looks like:

It appears as if both nodes have started. In the gazebo world, the turtlebot starts to move forward as expected. Now the terminal does print the lines "saved image..." and "received", however, the image taken is not being saved in the directory.

The fact that the line "received" is printed means that it is going through the cv2.imwrite line, why then is the image not being saved?

This wasn't a problem when I ran that python program individually (without the launchfile), so maybe the launchfile is interfering with the image saving process....what changes should I make to the launchfile if this is the case?

Combining two nodes using a launch-file , and cv2.imwrite() not working as a result

Answers (1)

Related Questions