def listener(self):
rospy.init_node('listenerLidar', anonymous=True)
self.move = simple_navigation_goals.SimpleNavigationGoals()
rospy.on_shutdown(self.move._shutdown)
self.TIMER_START = rospy.Time.now()
self.timer_unspam = rospy.Time.now()
self.SUBSCRIBED_POSE_TOPIC = rospy.Subscriber(
'/amcl_pose', PoseWithCovarianceStamped, self.save_initial_pose)
self.SUBSCRIBED_LIDAR_TOPIC = rospy.Subscriber('/scan', LaserScan,
self.callback)
rospy.spin()
def main():
global move_base_simple
rospy.init_node("tb3_labyrinthe")
rospy.loginfo("ParticleFilterHandler Initialization")
particle_handler = particle_filter_handler.ParticleFilterHandler()
rospy.loginfo("SimpleNavigationGoals Initialization")
nav_goals = simple_navigation_goals.SimpleNavigationGoals()
rospy.loginfo("Initializations done")
# What to do if shut down (e.g. ctrl + C or failure)
rospy.on_shutdown(nav_goals._shutdown)
rate_1 = rospy.Rate(1)
rate_10 = rospy.Rate(10)
cube_param_name = "/ros4pro/label"
rotation_time = rospy.Duration(3)
cmd_vel_pub = rospy.Publisher("/tb3/cmd_vel", Twist, queue_size=10)
cmd_vel = Twist()
cmd_vel.angular.z = 0.7
while not rospy.is_shutdown():
# Waits for cube param to exist, get the value and then delete the param from the server
rospy.loginfo("Waiting for cube number")
cube_number = -1
while cube_number < 0:
if rospy.has_param(cube_param_name):
cube_number = rospy.get_param(cube_param_name)
rospy.loginfo("param {}: {}".format(cube_param_name,
cube_number))
rospy.delete_param(cube_param_name)
else:
rate_1.sleep()
ret = False
# Sends nav goal depending on cube number, if it reachs the goal ret is set to True
if cube_number == 1:
rospy.loginfo(
"Cube number {} received, going to drop location".format(
cube_number))
# DROP POINT 1
ret = nav_goals.go_to(0, 0.5, math.pi / 2)
elif cube_number == 2:
rospy.loginfo(
"Cube number {} received, going to drop location".format(
cube_number))
# DROP POINT 2
ret = nav_goals.go_to(-0.5, -0.3, 0)
else:
rospy.logwarn(
"Cube number {} received, number must be 1 and 2, resetting state"
)
continue # remaining code of the loop will not be executed
if not ret:
rospy.logerr(
"Failed to reach drop point {}, going back to Sawyer".format(
cube_number))
# SAWYER POINT
ret = nav_goals.go_to(1, 0, 0)
if not ret:
rospy.logfatal(
"Failed to reach Sawyer point after failing drop, exiting node"
)
return # exit
else:
rospy.logwarn("Sawyer reached, resetting state")
continue
else:
rospy.sleep(1)
start_time = rospy.Time.now()
while rospy.Time.now() - start_time < rotation_time:
cmd_vel_pub.publish(cmd_vel)
rospy.loginfo("drop")
rate_10.sleep()
rospy.sleep(1)
cmd_vel_pub.publish(Twist())
rospy.loginfo(
"Cube successfully dropped, going back to the sawyer")
# SAWYER POINT
ret = nav_goals.go_to(1, 0, 0)
if not ret:
rospy.logfatal("Failed to reach Sawyer point, exiting node")
return # exit
rospy.init_node("listener", anonymous=False)
rospy.Subscriber("/scan", LaserScan, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == "__main__":
try:
rospy.init_node("listener")
# pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
cmd_vel = rospy.Publisher("cmd_vel", Twist, queue_size=10)
rospy.loginfo("SimpleNavigationGoals Initialization")
nav_goals = simple_navigation_goals.SimpleNavigationGoals()
rospy.loginfo("Initializations done")
client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
client.wait_for_server()
rospy.on_shutdown(nav_goals._shutdown)
# test le temps
sec = rospy.get_time()
print("Sec : {}".format(sec))
status = 0
target_linear_vel = 0.0
target_angular_vel = 0.0
control_linear_vel = 0.0