ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to reset odometry after setting /initialpose for amcl

asked 2022-05-29 12:11:57 -0500

btech gravatar image

updated 2022-06-01 17:48:12 -0500

Hello, i'm training a robot for navigation with reinforcement learning in gazebo. After each episode i teleport the robot to a new pose using the function get_free_pose and teleport_robot bellow and then publish the current pose to amcl. Now i need a way to reset the odometry data the ekf_localization node is subscribing to and set it to the current x,y pose that i published to amcl /initialpose. Any Idea how i can do that ?

def get_free_pose(self):
        poses = [(0,0),(0.8,0.3),(1.88,0.7),(-1.47,-1.05),(-1.24,-2.08),(-0.5,-2.12),(1.7,-1.5)]
        x,y = random.choice(poses)
        return x,y

def teleport_robot(self,pose):
    model_state_msg = ModelState()
    model_state_msg.model_name = "husky"

    model_state_msg.pose = pose
    model_state_msg.twist = Twist()
    model_state_msg.reference_frame = "world"

    # Start teleporting in Gazebo
    isTeleportSuccess = False
    for i in range(5):
        if not isTeleportSuccess:
            try:
                rospy.wait_for_service('/gazebo/set_model_state')
                self.telep_model_prox(model_state_msg)
                isTeleportSuccess = True
                break
            except Exception as e:
                rospy.logfatal("Error when teleporting agent " + str(e))
        else:
            rospy.logwarn("Trying to teleporting agent..." + str(i))
            time.sleep(2)

    if not isTeleportSuccess:
        rospy.logfatal("Error when teleporting agent")
        return "Err", "Err"`
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-06-07 20:53:20 -0500

Generally, odometry is calculated by the mobile base controller. You'd better check with the mobile base manufacturer whether they can provide a rosservice to reset the odometry. Or else you need to write a piece of code subscribing to the raw odometry and then publish a "compensate" odometry which can be reset.

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2022-05-29 12:11:57 -0500

Seen: 249 times

Last updated: Jun 07 '22