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