Hi, I 've got this when I finish the project
Why have I State 1 when I return the feedback (current_state)?
My server_action_client give a feedback when success it’s true. So why my result_state is 1?
In this case, my robot keep moving because result_state >=2 is False. Despite the feedback has given.
Also, my robot stop a few seconds later and return the feedback again, but this time es 9!
Hi @Voltedge ,
Okay, first things first, When you post an error that is not a general error that you can search on the internet, you need to add supporting documents like your program code.
Looking at your error, it is very clear that you have a problem in your code, and you have done it without your knowledge.
In most cases, if the error is absurd or very specific, it is the error on the program code made by the programmer (which is you!).
If you need help fixing this, you need to provide your code so that we can help you out.
Please tell us what you are working on. From the output of your terminal I am sure you are working on ROS1 Basics Project, but I am not sure if it is Python or C++.
Also, please avoid using words like “Help” in the title. It is obvious that if someone posts in the forum, they are looking for help. You do not have to state the obvious.
Cheers,
Girish
Hi @Voltedge ,
You need to put your code inside a code block.
Nobody will help you if you share your code which is “text” as an “image”.
Please learn how to insert a code block from here: Extended Syntax | Markdown Guide
And then repost your response as a “text”.
– Girish
Hi @girishkumar.kannan and sorry.
Action_server (record.py)
#! /usr/bin/env python
import rospy
import actionlib
from record_odom.msg import OdomRecordFeedback, OdomRecordResult, OdomRecordAction,
OdomRecordGoal
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point
from tf.transformations import euler_from_quaternio
from math import sqrt
############################ ACTION SERVER ######################################
class ReadOdomClass(object):
# create messages that are used to publish feedback/result
_feedback = OdomRecordFeedback() # Devuelve la distancia total actual
_result = OdomRecordResult() # Lista de odometrias
_goal = OdomRecordGoal() # String que dice si llegamos o no
def __init__(self):
self._odometry = {
'x':0.0,
'y':0.0,
'theta':0.0
}
# ACTION SERVER FOR NAV_MSGS -----> ODOMETRY
self._server_action = actionlib.SimpleActionServer('/record_odom', OdomRecordAction,
self.odom_record_callback, False)
self._server_action.start()
def _odom_callback(self, odom_msg):
self._odometry['x'] = odom_msg.pose.pose.position.x
self._odometry['y'] = odom_msg.pose.pose.position.y
orientation_q = odom_msg.pose.pose.orientation
(_, _, self._odometry["theta"]) = euler_from_quaternion([orientation_q.x,
orientation_q.y,
orientation_q.z,
orientation_q.w])
def odom_record_callback(self,goal):
odom_msg = rospy.Subscriber('/odom', Odometry, self._odom_callback, queue_size=1)
r = rospy.Rate(1)
success = False
self._feedback.current_total = 0.0 # Initializing the current_total
i = 0
while not success:
#Stop the goal_callback if exist a cancellation
if self._server_action.is_preempt_requested():
rospy.loginfo('The goal has been cancelled/preempted')
self._server_action.is_preempt_requested()
success = False
break
current_odom = Point()
current_odom.x = self._odometry["x"]
current_odom.y = self._odometry["y"]
current_odom.z = self._odometry["theta"]
self._result.list_of_odoms.append(current_odom)
if i <1:
self._feedback.current_total = 0.0
else:
# Distance
self._feedback.current_total += sqrt( pow( self._result.list_of_odoms[i].x - self._result.list_of_odoms[i-1].x, 2) +
pow( self._result.list_of_odoms[i].y - self._result.list_of_odoms[i-1].y, 2) )
if self._feedback.current_total > 5.9:
success = True
i +=1
self._server_action.publish_feedback(self._feedback)
r.sleep()
if success:
rospy.loginfo("Success getting odometry of the circuit")
self._server_action.set_succeeded(self._result)
if __name__ == '__main__' :
rospy.init_node('action_server', anonymous=True, log_level=rospy.INFO)
ReadOdomClass()
rospy.spin()Action_server_client and wall_follower
#! /usr/bin/env python
import rospy
import time
import enum
import actionlib
import sys
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from find_wall.srv import FindWall, FindWallRequest
from record_odom.msg import OdomRecordFeedback, OdomRecordAction, OdomRecordGoal
class Robot(object):
def __init__(self, mode):
rospy.loginfo("Robot has been called!")
self.move = Twist()
self.laser = {
"f_laser": 0.0,
"r_laser": 0.0,
"l_laser": 0.0
}
self._rate = rospy.Rate(10)
self._mode = mode.value
self.current_distance = 0.0
rospy.on_shutdown(self._stop)
self.end = False
# Subscribers and publishers
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.laser_sub = rospy.Subscriber('/scan', LaserScan, self._laser_callback)
self.action_client = actionlib.SimpleActionClient('/record_odom', OdomRecordAction)
def _laser_callback(self, scan):
laser_range = scan.ranges
if self._mode == 0:
self.laser["front_laser"] = min(laser_range[350:370])
self.laser["right_laser"] = min(laser_range[170:190])
self.laser["left_laser"] = min(laser_range[530:550])
def _stop(self):
self.move.linear.x = 0.0
self.move.angular.z = 0.0
self.vel_pub.publish(self.move)
def time_to_move(self):
rospy.loginfo("Time to move! Advance and follow the wall")
self._stop()
# Wait for the action server
self.action_client.wait_for_server()
goal = OdomRecordGoal()
self.action_client.send_goal(goal, feedback_cb=self._feedback_callback)
state_result = self.action_client.get_state()
while not rospy.is_shutdown():
# + angular : left
# - angular : right
self.move.linear.x = 0.06
if (0.3 >= self.laser["right_laser"] >= 0.2) and (0.51 < self.laser["front_laser"]):
self.move.angular.z = 0.0
rospy.logdebug("Forward!")
elif 0.5 > self.laser["front_laser"]:
self.move.angular.z = 0.35
rospy.logdebug("Corner!")
elif self.laser["right_laser"] > 0.3:
self.move.angular.z = -0.080
rospy.logdebug("Turn to right!")
elif self.laser["right_laser"] < 0.2:
self.move.angular.z = 0.085
rospy.logdebug("Turn to left!")
self.vel_pub.publish(self.move)
state_result = self.action_client.get_state()
rospy.logdebug(state_result)
self._rate.sleep()
if state_result >=2:
break
"""
PENDING = 0
ACTIVE = 1
DONE = 2
WARN = 3
ERROR = 4
"""
self._stop()
rospy.loginfo("[Result] State: " + str(state_result))
if state_result == 2: #DONE
rospy.logerr("Project done!")
if state_result == 3: #WARN
rospy.logerr("There is a warning in the Server Side")
if state_result == 4: #ERROR
rospy.logwarn("There is a error in the Server Side")
def _feedback_callback(self, feedback):
self.current_distance = feedback.current_total
rospy.loginfo("Current distance: " + str(feedback.current_total))
state_result = self.action_client.get_state()
rospy.loginfo("[Result] State: " + str(state_result))
if __name__ == '__main__' :
# Select the type of robot, simulation or real
class Sim_or_Real (enum.Enum):
Sim = 0
Real = 1
# Create a node
rospy.init_node("wall_follower", anonymous=True, log_level=rospy.INFO)
# Get the command line arguments from the launch file
robot_type = sys.argv[1]
rospy.wait_for_service("/find_wall")
wall_client = rospy.ServiceProxy('/find_wall', FindWall)
# Wait 5 seconds to call the service
time.sleep(5)
wall_client_objet = FindWallRequest()
result = wall_client(wall_client_objet)
rospy.loginfo(result)
if robot_type == "sim":
robot_control = Robot(mode=Sim_or_Real.Sim)
else:
robot_control = Robot(mode=Sim_or_Real.Real)
try:
if result:
robot_control.time_to_move()
except rospy.ROSInterruptException:
rospy.signal_shutdown("")Hi @Voltedge ,
After going through the code, I found that you are printing “[Result] State:” in the feedback callback which is not required.
So in your wall/_follower and action_server_client code, remove the last two lines:
def _feedback_callback(self, feedback):
self.current_distance = feedback.current_total
rospy.loginfo("Current distance: " + str(feedback.current_total))
# the below two lines are not needed
# state_result = self.action_client.get_state()
# rospy.loginfo("[Result] State: " + str(state_result))
Regards,
Girish
PS: I have completed this project. So, saying this from my experience.
Okay, I deleted these lines.
Is it okay that the robot keep moving for a 1-2 seconds, and then stop cause the result state = 9?
I just made only the changes that u said
That means when I print the result from callback the result is still 1, like in the first photo I sent, could you tell me why it doesn’t change to 2?
Hi @Voltedge ,
Apparently, your Goal is LOST! State 9 => LOST.
Refer here: common_msgs/GoalStatus.msg at noetic-devel · ros/common_msgs · GitHub
The action server successfully breaks out the loop when state >= 2, but since the state is 9, your client does not know that the goal is lost.
I am certain that your code has a problem. So when you call your action server using your action client program, I guess something happens on the action client side that it is unable to communicate with the server, although the server is (?) available.
The best way to debug your code is to make the action server and client separately and check if server and client code functions.
So do the following steps to check:
Once you get them working separately, you can integrate them into your main code.
Regards,
Girish
I tried to do what you said and it works!
state_result returns 3 now, I though it returns that because the action_server stops before/after the client and I tried both, but I have the same state_result = 3.
Some advice?
I just change one line code on time_to_move (while condition)
def time_to_move(self):
rospy.loginfo("Time to move! Advance and follow the wall")
self._stop()
# Wait for the action server
self.action_client.wait_for_server()
goal = OdomRecordGoal()
self.action_client.send_goal(goal, feedback_cb=self._feedback_callback)
state_result = self.action_client.get_state()
while self._feedback.current_total < 1.0:
# + angular : left
# - angular : right
self.move.linear.x = 0.06
if((0.3 >= self.laser["right_laser"] and self.laser["right_laser"] >= 0.2) and 0.51 < self.laser["front_laser"]):
self.move.angular.z = 0.0
rospy.logdebug("Forward!")
elif 0.5 > self.laser["front_laser"]:
self.move.angular.z = 0.35
rospy.logdebug("Corner!")
elif self.laser["right_laser"] > 0.3:
self.move.angular.z = -0.080
rospy.logdebug("Turn to right!")
elif self.laser["right_laser"] < 0.2:
self.move.angular.z = 0.085
rospy.logdebug("Turn to left!")
self.vel_pub.publish(self.move)
state_result = self.action_client.get_state()
rospy.logdebug(state_result)
self._rate.sleep()
if state_result >= 2:
break
"""
PENDING = 0
ACTIVE = 1
DONE = 2
WARN = 3
ERROR = 4
"""
self._stop()
state_result = self.action_client.get_state()
rospy.loginfo("[Result] State: " + str(state_result))
if state_result == 2: # DONE
rospy.logerr("Project done!")
if state_result == 3: # WARN
rospy.logerr("There is a warning in the Server Side")
if state_result == 4: # ERROR
rospy.logwarn("There is a error in the Server Side")
def _feedback_callback(self, feedback):
self.current_distance = feedback.current_total
rospy.loginfo("Current distance: " + str(feedback.current_total))
if __name__ == "__main__":
# Create a node
rospy.init_node("wall_follower", anonymous=True, log_level=rospy.INFO)
robot_control = Robot()
try:
robot_control.time_to_move()
robot_control._stop()
except rospy.ROSInterruptException:
rospy.signal_shutdown("")Hi @Voltedge ,
The good news is you have it working!
The warning is just stating that once the goal was complete, the action server completed the procedure.
This is clearly explained in the course tutorial. The paragraph on TCP/IP connection termination.
This is not something to worry about. But there is a way to get rid of this.
You can implement exception handling (try-except blocks) in python.
I am not going to tell you what to do, but a hint that I can provide you is: use try-except block in action server code.
You can also just ignore the warning and continue but that is your choice.
I also noticed something else in your code: your feedback is just printed once.
Are you updating the feedback every 1 second (as per the instructions)? So you must be getting a few feedback messages when the action is taking place.
You seem to have just one “reading” for the current_total, and that is right before the action is complete.
You need to implement the above logic correctly.
Regards,
Girish
Ohh I don’t think that it was a problem.
It’s part of my code hahaha
state_result = self.action_client.get_state()
rospy.loginfo("[Result] State: " + str(state_result))
if state_result == 2: # DONE
rospy.logerr("Project done!")
if state_result == 3: # WARN
rospy.logerr("There is a warning in the Server Side")
if state_result == 4: # ERROR
rospy.logwarn("There is a error in the Server Side")
I guess I change the message that prints the condition
Hi @Voltedge ,
It is not a part of “your” code, rather it is a part of the code given to you.
Also, the indication of warning is actually correct. So you must find a way to get rid of that error. And, it is not by deleting that line from the code.
I have retained all these lines in my project and just got rid of the warning.
Regards,
Girish
Hi, I use this but seems not working, on server_action code and I put the same in action_client, but nothing
Some advice?
if __name__ == '__main__':
try:
rospy.init_node('action_server', anonymous=True, log_level=rospy.INFO)
ReadOdomClass()
rospy.spin()
except rospy.ROSInterruptException:
passHi @Voltedge ,
Sure, you can use try-except block in the “__main__”, but I wanted you to use another try-except block in the action server callback function.
Try and experiment with your code and see what happens. I can only give you ideas. I cannot code for you!
Regards,
Girish
This topic was automatically closed 10 days after the last reply. New replies are no longer allowed.