error in ICP failed for some reason during use laser_base_scan in reinforcement learning

[yani-rl-ai]

HI HSR development support @yosuke @yuka-hashiguchi @nobuyuki-matsuno
i am working on HSR obstacle avoidance using Deep Reinfocement Learning, i use laser_scan on HSR to detect the obstacle.
However, i have a problem during training and reset the simulation world in gazebo, the error appear as follows :

poly_greatest_real_root: Could not find real root for polynomial.
polynomial coefficients : -0.108676 -3.848780 -36.713641 -46.699968 -16.000000
Roots:
root z0 = -0.058845680678141284 + +0.000000169915968913 i
root z1 = -0.058845680678141284 + -0.000000169915968913 i
root z2 = -1.400528318515152693 + +0.000000460218494630 i
root z3 = -1.400528318515152693 + -0.000000460218494630 i
:err: gpc_solve_valid failed
:err: icp_loop: Cannot compute next estimate.
:err: icp: ICP failed for some reason.
2020-11-25 14:25:59+0900 [-] [WARN] [1606281959.168205, 60.035000]: Unable to start server: Couldn't listen on any:9090: [Errno 98] Address already in use. Retrying in 5s.
:err: compute_next_estimate: something's fishy here! Old error: 0.501156 new error: 5.228942 x_old -0.199497 1.290519 -3.034672 x_new -0.341285 0.784230 -1.453939
:err: : before trimming, only 0 correspondences.
:err: icp: ICP failed for some reason.
:err: : before trimming, only 0 correspondences.
:err: icp: ICP failed for some reason.
:err: : before trimming, only 0 correspondences.
:err: icp: ICP failed for some reason.
:err: : before trimming, only 0 correspondences.
:err: icp: ICP failed for some reason.
:err: : before trimming, only 0 correspondences.
:err: icp: ICP failed for some reason.
:err: : before trimming, only 34 correspondences.
:err: icp: ICP failed for some reason.
.
.
.

i dont know how to solve this error, i have tried some recommendations from internet, but the error keep appear after the gazebo world reset in the looping program of reinforcement learning. i think this error because of the base_laser_scan keep estimating the pose. please kindly give us some suggestion.

[yosuke]

Hi,
This repository is published in OSS base (apart from TOYOTA official distribution) so our support will be in OSS style as well.
It seems the problem is on resetting the simulation. Some ROS nodes are not terminated correctly and causing the error on the next iteration.
If you can provide more details on how you reset the simulation, maybe we can help you.

[yani-rl-ai]

this is how i reset the simulation,
Thank you for your rapid answer

def reset(self):
rospy.wait_for_service('gazebo/reset_world')
try:
self.reset_proxy()
except (rospy.ServiceException) as e:
print("gazebo/reset_world service call failed")

    data = None
    while data is None:
        try:
            data = rospy.wait_for_message('/hsrb/base_scan'', LaserScan, timeout=5)
        except:
            pass

    if self.initGoal:
        self.goal_x, self.goal_y = self.respawn_goal.getPosition()
        self.initGoal = False

    self.goal_distance = self.getGoalDistace()
    state, done = self.getState(data)

    return np.asarray(state)

[yosuke]

Humm,
It seems only the gazebo is reset and the other ROS nodes are not reset.
This is probably the reason why the ICP process of laser scan matcher is showing the error.
Most of the ROS nodes do not support such a use case.

[yani-rl-ai]

Thanks for your respond, i tried the gym-gazebo for the use of Reinforcement Learning in gazebo with HSR. The default robot from gym-gazebo package is Turtlebot, i change all the topics and setting for HSR.
by using turtlebot, it worked very well, it does not have any problem. teh sensor reset very well

by the way, may i ask you, is it possible in HSR to visualize the laser scan in gazebo such as in below picture? i tried to custom the hokuyo.xacro file in HSR package, but it doesnt work

[yosuke]

If you are using gym-gazebo, you probably need to modify the launch file to launch only the ros_controllers and remove all the other ros nodes.
Visualization of LRF is possible. It will make us much easy to spot the problem, if you can share details of what you did to hokuyo.xacro.

[yani-rl-ai]

**Hi @yosuke thanks for kind reply.

here the program that i change in hokuyo laser scan**

****

<xacro:macro name="hokuyo_laser_gazebo" params="sensor_name robot_namespace ros_topic frame_name min_angle max_angle">
    <gazebo reference="${frame_name}">
        <sensor type="ray" name="${sensor_name}">
            <pose>0 0 0 0 0 0</pose>
            <update_rate>30.0</update_rate>
            <visualize>$(arg gazebo_visualization_enabled)</visualize>
            <ray>
                <scan>
                    <horizontal>
                        <samples>721</samples>
                        <resolution>1</resolution>
                        <min_angle>${min_angle}</min_angle>
                        <max_angle>${max_angle}</max_angle>
                    </horizontal>
                </scan>
                <range>
                    <min>0.05</min>
                    <max>60</max>
                    <resolution>0.001</resolution>
                </range>
            </ray>

            <plugin name="hokuyo_laser" filename="libgazebo_ros_laser.so">
                <topicName>${robot_namespace}/${ros_topic}</topicName>
                <frameName>${frame_name}</frameName>
            </plugin>
        </sensor>
    </gazebo>
</xacro:macro>

the default gazebo visualization is 'false', but i do not know when i change the in to 'true', the laser still not visualize
<xacro:arg name="gazebo_visualization_enabled" default="false" />

[yosuke]

I am not sure what you are trying to do... but our default for visualization is "true".
https://github.com/hsr-project/hsrb_description/blob/master/urdf/sensors/hokuyo_laser.gazebo.xacro#L3

The "default" attribute in "arg" tag will only define the default, and can be overwritten in the parent launch files.
See:
http://wiki.ros.org/roslaunch/XML/arg

Why don't you simply edit this line:

<visualize>$(arg gazebo_visualization_enabled)</visualize>

to this:

<visualize>true</visualize>

If it works, the value is overwritten somewhere in your launch files.

[yani-rl-ai]

aahh....i am so sorry, i just realize that line.

Many thanks for your guidance.

i successfully visualize the laser scan