Robotics StackExchange | Archived questions

Why the requests (python) in ros subscriber keeps posting the old messages?

I have a ROS subscriber which listens to the topic, creates some message and then uses python requests to post that data. I am using an OOP model, where I have created a class called MessageHolder with initial values x = 0.1 y = 0.1.

Whenever the topic triggers a message, this class calls its own callback function, sets x and y to be the first message from the topic (which is a Path message, so it consists of multiple PoseStamped messages). For this particular example, x = 0.1 y = 6.1. Here is the callback function of that class:

def callback(self, msg):
        if len(msg.poses) == 0:
            return
        rospy.loginfo("callback")
        self.x = msg.poses[0].pose.position.x
        self.y = msg.poses[1].pose.position.y
        rospy.loginfo("X : %f, Y : %f", self.x, self.y)
        for pose in msg.poses:
            if not isClose(pose.pose.orientation.z, self.th_x) or not isClose(pose.pose.orientation.w, self.th_y):
                self.th_x = pose.pose.orientation.z
                self.th_y = pose.pose.orientation.w
                self.distance_x = 100*abs(pose.pose.position.x - self.x)
                self.distance_y = 100*abs(pose.pose.position.y - self.y)
                self.distance = math.sqrt(self.distance_x**2 + self.distance_y**2)
                self.send_to_socket("rotate_direction")
                self.to_initial_orientation()
                self.send_to_socket("move_direction")
                rospy.loginfo("The distance is %f", self.distance)
                self.to_zero()
                self.x = pose.pose.position.x
                self.y = pose.pose.position.y

                time.sleep(2)
        self.to_initial_orientation()
        r = requests.post(path_url, json=self.path_msg)
        self.path_msg["path"] = []
        r.close()

So for the first time, everything works great and it posts a 1 path message to the server. Then after few minutes, with no messages being posted to the topic the subscriber is subscribed, I can see another path message being posted on a server (please note, from the logs, I didn't see that the callback above being called). The message is exactly the same except the initial values being x = 0.1, y = 0.1 (like during the initialisation) but the object has not been reinitalized! What is going on? Why does the requests keep posting?

Here is the full class and subscriber code:

#! /usr/bin/env python

import sys
import json
import ast
import time
import requests
from collections import OrderedDict
import rospy
import socketIO_client
import math
from nav_msgs.msg import Path
from nav_msgs.srv import GetPlan
from geometry_msgs.msg import PoseStamped 
from rosbridge_library.rosbridge_protocol import RosbridgeProtocol
import tf.transformations as T


path_url = "https://ros-bridge.herokuapp.com/p/"
host = "https://havit-web.herokuapp.com"
email = "edmund_@gmail.com"
wheelme_id = 4255
socketio = socketIO_client.SocketIO(host, params={"email": email})


class MessageHolder:
    x = -0.1
    y = -0.1
    th_x = 0.0
    th_y = 0.0
    last_x = 0.0
    last_y = 0.0
    last_yaw = 0.0
    current_yaw = 0.0
    wheelme_id = None
    command = None
    path_msg = {"path" : []}
    distance = 0.0
    distance_x = 0.0
    distance_y = 0.0
    start = None

    def __init__(self, x, y, yaw, wheelme_id):
        rospy.loginfo("%s","OBJECT HAS BEEN INITIALIZED")
        self.x = x
        self.y = y
        self.last_yaw = yaw
        self.current_yaw = yaw
        self.wheelme_id = wheelme_id
        self.command = ""
        self.start = self.create_msg_params()


    def prepare_message(self):
        if self.command == "move_direction":
            return {"wheelme_id" : self.wheelme_id, "arg" : {"direction" : "forward", "distance" : self.distance}}
        return {"wheelme_id" : self.wheelme_id, "arg" : {"direction" : self.get_direction(), "amount" : self.current_yaw}}

    def prepare_path_msg(self):
        rospy.loginfo("PRepare oth msg")
        return {"position" : {"x" :str(self.x), "y" : str(self.y)}, "orientation" : {"x" : str(self.last_yaw)}}

    def on_response_state(self, msg):
        battery = msg["state"]["raw"]["statuses"][0]["batt"]
        rospy.loginfo("The battery level is %f", battery)

    def add_path(self):
        msg = self.prepare_message()
        self.path_msg["path"].append(self.prepare_path_msg())

    def post_path_url(self):
        #socketIO.emit('receive_state_update', self.on_response_state())
        msg = self.prepare_message()
        return msg

    def get_direction(self):
        if self.current_yaw < 0:
            self.current_yaw *= -1
            return "RIGHT"
        return "LEFT"

    def convert_degrees(self):
        explicit_quat = [0,0,self.th_x,self.th_y]
        self.current_yaw = T.euler_from_quaternion(explicit_quat)[2]
        yaw = int(math.degrees(self.current_yaw))
        self.current_yaw = yaw - self.last_yaw
        self.last_yaw = yaw

    def send_to_socket(self, command):
        self.command = command
        if command == "rotate_direction":
          self.convert_degrees()
          self.add_path()
        msg = self.post_path_url()
        #socketio.emit(command, msg)

    def to_initial_orientation(self):
        msg = {"wheelme_id" : self.wheelme_id, "arg" : {"direction" : "one_wheel", "distance" : 2}}
        #socketio.emit("move_direction", msg)

    def to_zero(self):
        self.distance = 0.0
        self.distance_x = 0.0
        self.distance_y = 0.0

    def create_msg_params(self, pos_x=0, pos_y=0, orient_x=0, orient_y=0, orient_z=0, orient_w=1, _id=0):
        msg = PoseStamped()
        msg.header.seq = _id
        msg.header.stamp = rospy.get_time()
        msg.header.frame_id = 'map'
        msg.pose.position.x = pos_x
        msg.pose.position.y = pos_y
        msg.pose.position.z = 0.0
        msg.pose.orientation.x = orient_x
        msg.pose.orientation.y = orient_y
        msg.pose.orientation.z = orient_z
        msg.pose.orientation.w = orient_w
        return msg

    def call_service(self, end):
        rospy.wait_for_service('/move_base/make_plan')
        gp_service = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
        try:
          response = gp_service(self.start, end, 1.0)
          #rospy.loginfo("The path is %s", response.plan.poses)
          return response.plan
        except rospy.ServiceException as e:
          print("Service did not process request " + str(e))


    def callback(self, msg):
        if len(msg.poses) == 0:
            return
        rospy.loginfo("callback")
        self.x = msg.poses[0].pose.position.x
        self.y = msg.poses[1].pose.position.y
        rospy.loginfo("X : %f, Y : %f", self.x, self.y)
        for pose in msg.poses:
            if not isClose(pose.pose.orientation.z, self.th_x) or not isClose(pose.pose.orientation.w, self.th_y):
                self.th_x = pose.pose.orientation.z
                self.th_y = pose.pose.orientation.w
                self.distance_x = 100*abs(pose.pose.position.x - self.x)
                self.distance_y = 100*abs(pose.pose.position.y - self.y)
                self.distance = math.sqrt(self.distance_x**2 + self.distance_y**2)
                self.send_to_socket("rotate_direction")
                self.to_initial_orientation()
                self.send_to_socket("move_direction")
                rospy.loginfo("The distance is %f", self.distance)
                self.to_zero()
                self.x = pose.pose.position.x
                self.y = pose.pose.position.y

                time.sleep(2)
        self.to_initial_orientation()
        #self.start = poses.poses[-1]
        #rospy.loginfo("The starting X is : %f, starting Y : %f", self.start.pose.position.x, self.start.pose.position.y)
        r = requests.post(path_url, json=self.path_msg)
        r.close()
        self.path_msg["path"] = []


def on_connect(*args):
    rospy.loginfo("socket.io connected")

def on_disconnect(*args):
    rospy.loginfo("socket.io disconnected")

def isClose(a, b):
    return str(a) == str(b)

def check_length(data):
    return len(data) != 0

def node_start():
    #socketIO.on('connect', on_connect)
    #socketIO.on('disconnect', on_disconnect)
    global wheelme_id
    rospy.init_node('heroku_socket')
    msg_holder = MessageHolder(-0.1,-0.1,0.0,wheelme_id)
    rospy.Subscriber('move_base/GlobalPlanner/plan', Path, msg_holder.callback)
    rospy.spin()


if __name__ == "__main__":
        node_start()

Asked by stevemartin on 2018-12-06 05:55:36 UTC

Comments

Answers

If this callback is definitely not being called, then this looks like it is an issue with the http interface you've made in your class so technically not a ROS problem. Without seeing the rest of the class there's not much we can do.

Note, there's an unrelated minor bug in your code: Getting the initial point on the path should be:

    self.x = msg.poses[0].pose.position.x
    self.y = msg.poses[0].pose.position.y

UPDATE:

The only thing I can think is that several path messages are being received at the very start and several callbacks are being executed. The unusual thing about the callback has a time.sleep(2) inside the loop meaning that it could easily take minutes to finish executing for a path with 60+ way-points. Try removing the sleep call and see how it behaves then.

Asked by PeteBlackerThe3rd on 2018-12-06 06:31:11 UTC

Comments

@PeteBlackerThe3rd Please see my edit, I have added the full code

Asked by stevemartin on 2018-12-06 06:55:50 UTC

Cheers, see my update.

Asked by PeteBlackerThe3rd on 2018-12-06 07:07:47 UTC

@PeteBlackerThe3rd Yes, the issue still exists. Why do you think that the callback is being called several times instead of 1? I only read the rospy.loginfo("callback") only once.

Asked by stevemartin on 2018-12-06 07:48:41 UTC

Simply because the only occurrence of requests.post is in the callback function, so it's the only explanation for another message being received on the server. Unless it's possible more than one instance of this node is running at the same time?

Asked by PeteBlackerThe3rd on 2018-12-06 08:27:05 UTC

You could try adding a sequence number into the path message that is POSTed to your server to help work out where they're coming from.

Asked by PeteBlackerThe3rd on 2018-12-06 08:29:13 UTC