更改车辆模型 Change Vehicle Model

[peipei518]

Thank you for your project. I would like to replace the robot model with my own four-wheel differential model, but I have found that when my robot collides after the replacement, the environment will not reset. I would like to ask the following question: 1. Is the collision detected in your code using radar point cloud data? I found that after replacing the model, I cannot find the topic of point cloud data in rviz, Could it be because of this that collisions cannot be detected. 2. Could it be some other problem that caused this result? 3.//velodyne_points Where did the points topic come from? After I changed the vehicle model, rostopic list was able to find this topic, but I cannot find it in rviz, and I am not using the radar xacro file from your project. Why is this?If possible, could you give me some suggestions.What work does it take to modify the vehicle model based on your project The picture shows the situation of my robot after collision

[peipei518]

Sorry, the previous screenshot was uploaded incorrectly. This is my screenshot. After the collision, the rviz small and medium-sized car continued to move, and the radar data also shifted

[reiniscimurs]

Hi,

What kind of changes did you make to the code itself? And what version of the repo are you using? If you changed the model and implementation, I would not be able to tell you where the sensor information comes from.

Yes, by default we use the velodyne lidar to detect collisions. If you have not connected this lidar to your robot, it will not be able to detect collisions. https://github.com/reiniscimurs/DRL-robot-navigation/blob/main/TD3/velodyne_env.py#L182

What you can do is use your laser data as input to the collision detection function and update collision distance.

[peipei518]

Thank you for your reply. I am using ros Noetic and tried to replace the radar with the original one, but there was still a collision without resetting the environment.I am still unable to choose the /velodyne_points topic in rviz, This may be due to my lack of understanding of the code. Here are the modifications I have made. If possible, could you please help me take a look at the problem. import math import os import random import subprocess import time from os import path

import numpy as np import rospy import sensor_msgs.point_cloud2 as pc2 from gazebo_msgs.msg import ModelState from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from sensor_msgs.msg import PointCloud2 from squaternion import Quaternion from std_srvs.srv import Empty from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray

GOAL_REACHED_DIST = 0.4

GOAL_REACHED_DIST = 0.3

COLLISION_DIST = 0.5

COLLISION_DIST = 0.35 TIME_DELTA = 0.1

Check if the random goal position is located on an obstacle and do not accept it if it is

def check_pos(x, y): goal_ok = True

if -3.8 > x > -6.2 and 6.2 > y > 3.8:
    goal_ok = False

if -1.3 > x > -2.7 and 4.7 > y > -0.2:
    goal_ok = False

if -0.3 > x > -4.2 and 2.7 > y > 1.3:
    goal_ok = False

if -0.8 > x > -4.2 and -2.3 > y > -4.2:
    goal_ok = False

if -1.3 > x > -3.7 and -0.8 > y > -2.7:
    goal_ok = False

if 4.2 > x > 0.8 and -1.8 > y > -3.2:
    goal_ok = False

if 4 > x > 2.5 and 0.7 > y > -3.2:
    goal_ok = False

if 6.2 > x > 3.8 and -3.3 > y > -4.2:
    goal_ok = False

if 4.2 > x > 1.3 and 3.7 > y > 1.5:
    goal_ok = False

if -3.0 > x > -7.2 and 0.5 > y > -1.5:
    goal_ok = False

if x > 4.5 or x < -4.5 or y > 4.5 or y < -4.5:
    goal_ok = False

return goal_ok

class GazeboEnv: """Superclass for all Gazebo environments."""

def __init__(self, launchfile, environment_dim):
    self.environment_dim = environment_dim
    self.odom_x = 0
    self.odom_y = 0

    self.goal_x = 1
    self.goal_y = 0.0

    self.upper = 5.0
    self.lower = -5.0
    self.velodyne_data = np.ones(self.environment_dim) * 10
    self.last_odom = None

    self.set_self_state = ModelState()
    # self.set_self_state.model_name = "r1"
    self.set_self_state.model_name = "autolabor_pro1"
    self.set_self_state.pose.position.x = 0.0
    self.set_self_state.pose.position.y = 0.0
    self.set_self_state.pose.position.z = 0.0
    self.set_self_state.pose.orientation.x = 0.0
    self.set_self_state.pose.orientation.y = 0.0
    self.set_self_state.pose.orientation.z = 0.0
    self.set_self_state.pose.orientation.w = 1.0

    self.gaps = [[-np.pi / 2 - 0.03, -np.pi / 2 + np.pi / self.environment_dim]]
    for m in range(self.environment_dim - 1):
        self.gaps.append(
            [self.gaps[m][1], self.gaps[m][1] + np.pi / self.environment_dim]
        )
    self.gaps[-1][-1] += 0.03

    port = "11311"
    subprocess.Popen(["roscore", "-p", port])

    print("Roscore launched!")

    # Launch the simulation with the given launchfile name
    rospy.init_node("gym", anonymous=True)
    if launchfile.startswith("/"):
        fullpath = launchfile
    else:
        fullpath = os.path.join(os.path.dirname(__file__), "assets", launchfile)
    if not path.exists(fullpath):
        raise IOError("File " + fullpath + " does not exist")

    subprocess.Popen(["roslaunch", "-p", port, fullpath])
    print("Gazebo launched!")

    # Set up the ROS publishers and subscribers
    self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
    # self.vel_pub = rospy.Publisher("/r1/cmd_vel", Twist, queue_size=1)
    self.set_state = rospy.Publisher(
        "gazebo/set_model_state", ModelState, queue_size=10
    )
    self.unpause = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
    self.pause = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
    self.reset_proxy = rospy.ServiceProxy("/gazebo/reset_world", Empty)
    self.publisher = rospy.Publisher("goal_point", MarkerArray, queue_size=3)
    self.publisher2 = rospy.Publisher("linear_velocity", MarkerArray, queue_size=1)
    self.publisher3 = rospy.Publisher("angular_velocity", MarkerArray, queue_size=1)
    self.velodyne = rospy.Subscriber(
        "/velodyne_points", PointCloud2, self.velodyne_callback, queue_size=1
    )
    self.odom = rospy.Subscriber(
        # "/r1/odom", Odometry, self.odom_callback, queue_size=1
        "/odom", Odometry, self.odom_callback, queue_size=1
    )

# Read velodyne pointcloud and turn it into distance data, then select the minimum value for each angle
# range as state representation
def velodyne_callback(self, v):
    data = list(pc2.read_points(v, skip_nans=False, field_names=("x", "y", "z")))
    self.velodyne_data = np.ones(self.environment_dim) * 10
    for i in range(len(data)):
        if data[i][2] > -0.2:
            dot = data[i][0] * 1 + data[i][1] * 0
            mag1 = math.sqrt(math.pow(data[i][0], 2) + math.pow(data[i][1], 2))
            mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))
            beta = math.acos(dot / (mag1 * mag2)) * np.sign(data[i][1])
            dist = math.sqrt(data[i][0] ** 2 + data[i][1] ** 2 + data[i][2] ** 2)

            for j in range(len(self.gaps)):
                if self.gaps[j][0] <= beta < self.gaps[j][1]:
                    self.velodyne_data[j] = min(self.velodyne_data[j], dist)
                    break

def odom_callback(self, od_data):
    self.last_odom = od_data

# Perform an action and read a new state
def step(self, action):
    target = False

    # Publish the robot action
    vel_cmd = Twist()
    vel_cmd.linear.x = action[0]
    vel_cmd.angular.z = action[1]
    self.vel_pub.publish(vel_cmd)
    self.publish_markers(action)

    rospy.wait_for_service("/gazebo/unpause_physics")
    try:
        self.unpause()
    except (rospy.ServiceException) as e:
        print("/gazebo/unpause_physics service call failed")

    # propagate state for TIME_DELTA seconds
    time.sleep(TIME_DELTA)

    rospy.wait_for_service("/gazebo/pause_physics")
    try:
        pass
        self.pause()
    except (rospy.ServiceException) as e:
        print("/gazebo/pause_physics service call failed")

    # read velodyne laser state
    done, collision, min_laser = self.observe_collision(self.velodyne_data)
    v_state = []
    v_state[:] = self.velodyne_data[:]
    laser_state = [v_state]

    # Calculate robot heading from odometry data
    self.odom_x = self.last_odom.pose.pose.position.x
    self.odom_y = self.last_odom.pose.pose.position.y
    quaternion = Quaternion(
        self.last_odom.pose.pose.orientation.w,
        self.last_odom.pose.pose.orientation.x,
        self.last_odom.pose.pose.orientation.y,
        self.last_odom.pose.pose.orientation.z,
    )
    euler = quaternion.to_euler(degrees=False)
    angle = round(euler[2], 4)

    # Calculate distance to the goal from the robot
    distance = np.linalg.norm(
        [self.odom_x - self.goal_x, self.odom_y - self.goal_y]
    )

    # Calculate the relative angle between the robots heading and heading toward the goal
    skew_x = self.goal_x - self.odom_x
    skew_y = self.goal_y - self.odom_y
    dot = skew_x * 1 + skew_y * 0
    mag1 = math.sqrt(math.pow(skew_x, 2) + math.pow(skew_y, 2))
    mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))
    beta = math.acos(dot / (mag1 * mag2))
    if skew_y < 0:
        if skew_x < 0:
            beta = -beta
        else:
            beta = 0 - beta
    theta = beta - angle
    if theta > np.pi:
        theta = np.pi - theta
        theta = -np.pi - theta
    if theta < -np.pi:
        theta = -np.pi - theta
        theta = np.pi - theta

    # Detect if the goal has been reached and give a large positive reward
    if distance < GOAL_REACHED_DIST:
        target = True
        done = True

    robot_state = [distance, theta, action[0], action[1]]
    state = np.append(laser_state, robot_state)
    reward = self.get_reward(target, collision, action, min_laser)
    return state, reward, done, target

def reset(self):

    # Resets the state of the environment and returns an initial observation.
    rospy.wait_for_service("/gazebo/reset_world")
    try:
        self.reset_proxy()

    except rospy.ServiceException as e:
        print("/gazebo/reset_simulation service call failed")

    angle = np.random.uniform(-np.pi, np.pi)
    quaternion = Quaternion.from_euler(0.0, 0.0, angle)
    object_state = self.set_self_state

    x = 0
    y = 0
    position_ok = False
    while not position_ok:
        x = np.random.uniform(-4.5, 4.5)
        y = np.random.uniform(-4.5, 4.5)
        position_ok = check_pos(x, y)
    object_state.pose.position.x = x
    object_state.pose.position.y = y
    # object_state.pose.position.z = 0.
    object_state.pose.orientation.x = quaternion.x
    object_state.pose.orientation.y = quaternion.y
    object_state.pose.orientation.z = quaternion.z
    object_state.pose.orientation.w = quaternion.w
    self.set_state.publish(object_state)

    self.odom_x = object_state.pose.position.x
    self.odom_y = object_state.pose.position.y

    # set a random goal in empty space in environment
    self.change_goal()
    # randomly scatter boxes in the environment
    self.random_box()
    self.publish_markers([0.0, 0.0])

    rospy.wait_for_service("/gazebo/unpause_physics")
    try:
        self.unpause()
    except (rospy.ServiceException) as e:
        print("/gazebo/unpause_physics service call failed")

    time.sleep(TIME_DELTA)

    rospy.wait_for_service("/gazebo/pause_physics")
    try:
        self.pause()
    except (rospy.ServiceException) as e:
        print("/gazebo/pause_physics service call failed")
    v_state = []
    v_state[:] = self.velodyne_data[:]
    laser_state = [v_state]

    distance = np.linalg.norm(
        [self.odom_x - self.goal_x, self.odom_y - self.goal_y]
    )

    skew_x = self.goal_x - self.odom_x
    skew_y = self.goal_y - self.odom_y

    dot = skew_x * 1 + skew_y * 0
    mag1 = math.sqrt(math.pow(skew_x, 2) + math.pow(skew_y, 2))
    mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))
    beta = math.acos(dot / (mag1 * mag2))

    if skew_y < 0:
        if skew_x < 0:
            beta = -beta
        else:
            beta = 0 - beta
    theta = beta - angle

    if theta > np.pi:
        theta = np.pi - theta
        theta = -np.pi - theta
    if theta < -np.pi:
        theta = -np.pi - theta
        theta = np.pi - theta

    robot_state = [distance, theta, 0.0, 0.0]
    state = np.append(laser_state, robot_state)
    return state

def change_goal(self):
    # Place a new goal and check if its location is not on one of the obstacles
    if self.upper < 10:
        self.upper += 0.004
    if self.lower > -10:
        self.lower -= 0.004

    goal_ok = False

    while not goal_ok:
        self.goal_x = self.odom_x + random.uniform(self.upper, self.lower)
        self.goal_y = self.odom_y + random.uniform(self.upper, self.lower)
        goal_ok = check_pos(self.goal_x, self.goal_y)

def random_box(self):
    # Randomly change the location of the boxes in the environment on each reset to randomize the training
    # environment
    for i in range(4):
        name = "cardboard_box_" + str(i)

        x = 0
        y = 0
        box_ok = False
        while not box_ok:
            x = np.random.uniform(-6, 6)
            y = np.random.uniform(-6, 6)
            box_ok = check_pos(x, y)
            distance_to_robot = np.linalg.norm([x - self.odom_x, y - self.odom_y])
            distance_to_goal = np.linalg.norm([x - self.goal_x, y - self.goal_y])
            if distance_to_robot < 1.5 or distance_to_goal < 1.5:
                box_ok = False
        box_state = ModelState()
        box_state.model_name = name
        box_state.pose.position.x = x
        box_state.pose.position.y = y
        box_state.pose.position.z = 0.0
        box_state.pose.orientation.x = 0.0
        box_state.pose.orientation.y = 0.0
        box_state.pose.orientation.z = 0.0
        box_state.pose.orientation.w = 1.0
        self.set_state.publish(box_state)

def publish_markers(self, action):
    # Publish visual data in Rviz
    markerArray = MarkerArray()
    marker = Marker()
    marker.header.frame_id = "odom"
    marker.type = marker.CYLINDER
    marker.action = marker.ADD
    marker.scale.x = 0.1
    marker.scale.y = 0.1
    marker.scale.z = 0.01
    marker.color.a = 1.0
    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.pose.orientation.w = 1.0
    marker.pose.position.x = self.goal_x
    marker.pose.position.y = self.goal_y
    marker.pose.position.z = 0

    markerArray.markers.append(marker)

    self.publisher.publish(markerArray)

    markerArray2 = MarkerArray()
    marker2 = Marker()
    marker2.header.frame_id = "odom"
    marker2.type = marker.CUBE
    marker2.action = marker.ADD
    marker2.scale.x = abs(action[0])
    marker2.scale.y = 0.1
    marker2.scale.z = 0.01
    marker2.color.a = 1.0
    marker2.color.r = 1.0
    marker2.color.g = 0.0
    marker2.color.b = 0.0
    marker2.pose.orientation.w = 1.0
    marker2.pose.position.x = 5
    marker2.pose.position.y = 0
    marker2.pose.position.z = 0

    markerArray2.markers.append(marker2)
    self.publisher2.publish(markerArray2)

    markerArray3 = MarkerArray()
    marker3 = Marker()
    marker3.header.frame_id = "odom"
    marker3.type = marker.CUBE
    marker3.action = marker.ADD
    marker3.scale.x = abs(action[1])
    marker3.scale.y = 0.1
    marker3.scale.z = 0.01
    marker3.color.a = 1.0
    marker3.color.r = 1.0
    marker3.color.g = 0.0
    marker3.color.b = 0.0
    marker3.pose.orientation.w = 1.0
    marker3.pose.position.x = 5
    marker3.pose.position.y = 0.2
    marker3.pose.position.z = 0

    markerArray3.markers.append(marker3)
    self.publisher3.publish(markerArray3)

@staticmethod
def observe_collision(laser_data):
    # Detect a collision from laser data
    min_laser = min(laser_data)
    if min_laser < COLLISION_DIST:
        return True, True, min_laser
    return False, False, min_laser

@staticmethod
def get_reward(target, collision, action, min_laser):
    if target:
        return 100.0
    elif collision:
        return -100.0
    else:
        r3 = lambda x: 1 - x if x < 1 else 0.0
        return action[0] / 2 - abs(action[1]) / 2 - r3(min_laser) / 2

In this code file, I only modified the vehicle name and cmd_ The topics of vel and odom have not been modified

[peipei518]

This is my robot launch file

[peipei518]

This is my robot xacro file, where I replaced the radar with the radar in your project, but the above problem still occurs <?xml version="1.0"?>

[peipei518]

I don't know why a few lines of code were lost during upload, but the main code is there. If possible, could you help me check where the problem occurred

[reiniscimurs]

Your collision detection is still based on velodyne lidar input: # read velodyne laser state done, collision, min_laser = self.observe_collision(self.velodyne_data)

You have not attatched velodyne to your robot so collisions will not be detected. If you want to just use the hokuyo laser, then you will have to read the laser state from this hokuyo lidar instead.

On P3DX robot the velodyne laser is added here: https://github.com/reiniscimurs/DRL-robot-navigation/blob/main/catkin_ws/src/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro#L26-L29

[peipei518]

Thank you for your reply. I have resolved the issue of changing the vehicle model radar topic, but there are currently some other issues. Could you please give me some suggestions again

1. My radar data seems to shift while the car is moving. When I change the Fixed Frame to World, it seems that the radar does not shift, but the target point will move with the car, which I believe is abnormal. Here is my tf Tree and rqt Graph.The first two images show the selection of Odom and World for Fixed Frame 2.Due to my larger vehicle model, I attempted to change the COLLISION The value of DIST has been increased, but there has been a constant reset. I found in your other replies that it seems to be a def check The issue with pos (x, y), but I don't know the basis for setting the parameters in this function, so I don't know how to change it< Box size="0.7248 0.4302 0.195"/>These are the parameters of my chassis. Could you please give me some suggestions.

[reiniscimurs]

1. This would most likely be something dependent on your implementation and setup between the robot and the world. If setting fixed frame to world works for you, you can try publishing the goal points and markers in that frame as well. By default they are published in odom frame : https://github.com/reiniscimurs/DRL-robot-navigation/blob/main/TD3/velodyne_env.py#L366

2. You collision is quite obviously detected from your own robot. Meaning that the laser detects your own robot base, and since the laser values are so close, reset is triggered. The function check_pos will not have any influence on this and is used only for placing a random goal position. It has nothing to do with triggering reset. I do not see the robot model clearly, but it seems that you have also placed velodyne lidar sensor on top of your 2D lidar. This could also cause collisions to be detected. In any case, the issue is caused due to suboptimal placement of your lidar sensors.

I would suggest going through the code and really understanding what each function does in detail. It will be really difficult making changes without understanding what parts of the code take care of what. There is a code guide provided and it should help you understand the meaning of functions and their use.

[peipei518]

hi I am using a branch of Melodic With your suggestion, I set the fixed frame to world and made changes in the code, as well as changing the position of the 3D LiDAR. Now the collision can reset the environment normally. Due to my larger robot model, I changed the min in the code Range set to 0.6, Dist<0.6. Now my radar data and target point do not follow the deviation of the car, but I have found that the TF of the odom will shift. I cannot understand the reason for this. Now, my car passes through the target point but does not reset the environment. I suspect it is due to the deviation of the odom. My tf The tree is the same as above. If possible, could you please give me some suggestions? Thank you again! Here is my modified velodyne_ Env.py code， import rospy import subprocess from os import path from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray from numpy import inf import numpy as np import random import math from gazebo_msgs.msg import ModelState from squaternion import Quaternion from geometry_msgs.msg import Twist from sensor_msgs.msg import LaserScan, PointCloud2 import sensor_msgs.point_cloud2 as pc2 from nav_msgs.msg import Odometry from std_srvs.srv import Empty import os import time

os.environ['TF_CPP_MIN_LOG_LEVEL'] = '3'

Check if the random goal position is located on an obstacle and do not accept it if it is

def check_pos(x, y): goalOK = True

if -3.8 > x > -6.2 and 6.2 > y > 3.8:
    goalOK = False

if -1.3 > x > -2.7 and 4.7 > y > -0.2:
    goalOK = False

if -0.3 > x > -4.2 and 2.7 > y > 1.3:
    goalOK = False

if -0.8 > x > -4.2 and -2.3 > y > -4.2:
    goalOK = False

if -1.3 > x > -3.7 and -0.8 > y > -2.7:
    goalOK = False

if 4.2 > x > 0.8 and -1.8 > y > -3.2:
    goalOK = False

if 4 > x > 2.5 and 0.7 > y > -3.2:
    goalOK = False

if 6.2 > x > 3.8 and -3.3 > y > -4.2:
    goalOK = False

if 4.2 > x > 1.3 and 3.7 > y > 1.5:
    goalOK = False

if -3.0 > x > -7.2 and 0.5 > y > -1.5:
    goalOK = False

if x > 4.5 or x < -4.5 or y > 4.5 or y < -4.5:
    goalOK = False
return goalOK

Function to put the laser data in bins

def binning(lower_bound, data, quantity): width = round(len(data) / quantity) quantity -= 1 bins = [] for low in range(lower_bound, lower_bound + quantity * width + 1, width): bins.append(min(data[low:low + width])) return np.array([bins])

class GazeboEnv: """Superclass for all Gazebo environments. """ metadata = {'render.modes': ['human']}

def __init__(self, launchfile, height, width, nchannels):

    self.odomX = 0
    self.odomY = 0

    self.goalX = 1
    self.goalY = 0.0

    self.upper = 5.0
    self.lower = -5.0
    self.velodyne_data = np.ones(20) * 10

    self.set_self_state = ModelState()
    self.set_self_state.model_name = 'autolabor_pro1'
    # self.set_self_state.model_name = 'r1'
    self.set_self_state.pose.position.x = 0.
    self.set_self_state.pose.position.y = 0.
    self.set_self_state.pose.position.z = 0.
    self.set_self_state.pose.orientation.x = 0.0
    self.set_self_state.pose.orientation.y = 0.0
    self.set_self_state.pose.orientation.z = 0.0
    self.set_self_state.pose.orientation.w = 1.0
    self.distOld = math.sqrt(math.pow(self.odomX - self.goalX, 2) + math.pow(self.odomY - self.goalY, 2))
    self.gaps = [[-1.6, -1.57 + 3.14 / 20]]
    for m in range(19):
        self.gaps.append([self.gaps[m][1], self.gaps[m][1] + 3.14 / 20])
    self.gaps[-1][-1] += 0.03

    port = '11311'
    subprocess.Popen(["roscore", "-p", port])

    print("Roscore launched!")

    # Launch the simulation with the given launchfile name
    rospy.init_node('gym', anonymous=True)
    if launchfile.startswith("/"):
        fullpath = launchfile
    else:
        fullpath = os.path.join(os.path.dirname(__file__), "assets", launchfile)
    if not path.exists(fullpath):
        raise IOError("File " + fullpath + " does not exist")

    subprocess.Popen(["roslaunch", "-p", port, fullpath])
    print("Gazebo launched!")

    self.gzclient_pid = 0

    # Set up the ROS publishers and subscribers
    self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    # self.vel_pub = rospy.Publisher('/r1/cmd_vel', Twist, queue_size=1)
    self.set_state = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=10)
    self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
    self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
    self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_world', Empty)
    topic = 'vis_mark_array'
    self.publisher = rospy.Publisher(topic, MarkerArray, queue_size=3)
    topic2 = 'vis_mark_array2'
    self.publisher2 = rospy.Publisher(topic2, MarkerArray, queue_size=1)
    topic3 = 'vis_mark_array3'
    self.publisher3 = rospy.Publisher(topic3, MarkerArray, queue_size=1)
    topic4 = 'vis_mark_array4'
    self.publisher4 = rospy.Publisher(topic4, MarkerArray, queue_size=1)
    self.velodyne = rospy.Subscriber('/velodyne_points', PointCloud2, self.velodyne_callback, queue_size=1)

# Read velodyne pointcloud and turn it into distance data, then select the minimum value for each angle
# range as state representation
def velodyne_callback(self, v):
    data = list(pc2.read_points(v, skip_nans=False, field_names=("x", "y", "z")))
    self.velodyne_data = np.ones(20) * 10
    for i in range(len(data)):
        if data[i][2] > -0.2:
            dot = data[i][0] * 1 + data[i][1] * 0
            mag1 = math.sqrt(math.pow(data[i][0], 2) + math.pow(data[i][1], 2))
            mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))
            beta = math.acos(dot / (mag1 * mag2)) * np.sign(data[i][1])  # * -1
            dist = math.sqrt(data[i][0] ** 2 + data[i][1] ** 2 + data[i][2] ** 2)

            for j in range(len(self.gaps)):
                if self.gaps[j][0] <= beta < self.gaps[j][1]:
                    self.velodyne_data[j] = min(self.velodyne_data[j], dist)
                    break

# Detect a collision from laser data
def calculate_observation(self, data):
    min_range = 0.6
    min_laser = 2
    done = False
    col = False

    for i, item in enumerate(data.ranges):
        if min_laser > data.ranges[i]:
            min_laser = data.ranges[i]
        if (min_range > data.ranges[i] > 0):
            done = True
            col = True
    return done, col, min_laser

# Perform an action and read a new state
def step(self, act):
    target = False

    # Publish the robot action
    vel_cmd = Twist()
    vel_cmd.linear.x = act[0]
    vel_cmd.angular.z = act[1]
    self.vel_pub.publish(vel_cmd)

    rospy.wait_for_service('/gazebo/unpause_physics')
    try:
        self.unpause()
    except (rospy.ServiceException) as e:
        print("/gazebo/unpause_physics service call failed")

    data = None
    while data is None:
        try:
            # data = rospy.wait_for_message('/r1/front_laser/scan', LaserScan, timeout=0.5)
            data = rospy.wait_for_message('/scan', LaserScan, timeout=0.5)
        except:
            pass

    laser_state = np.array(data.ranges[:])
    v_state = []
    v_state[:] = self.velodyne_data[:]
    laser_state = [v_state]

    done, col, min_laser = self.calculate_observation(data)

    dataOdom = None
    while dataOdom is None:
        try:
            # dataOdom = rospy.wait_for_message('/r1/odom', Odometry, timeout=0.5)
            dataOdom = rospy.wait_for_message('/odom', Odometry, timeout=0.5)
        except:
            pass
    time.sleep(0.1)
    rospy.wait_for_service('/gazebo/pause_physics')
    try:
        pass
        self.pause()
    except (rospy.ServiceException) as e:
        print("/gazebo/pause_physics service call failed")

    # Calculate robot heading from odometry data
    self.odomX = dataOdom.pose.pose.position.x
    self.odomY = dataOdom.pose.pose.position.y
    quaternion = Quaternion(
        dataOdom.pose.pose.orientation.w,
        dataOdom.pose.pose.orientation.x,
        dataOdom.pose.pose.orientation.y,
        dataOdom.pose.pose.orientation.z)
    euler = quaternion.to_euler(degrees=False)
    angle = round(euler[2], 4)

    # Calculate distance to the goal from the robot
    Dist = math.sqrt(math.pow(self.odomX - self.goalX, 2) + math.pow(self.odomY - self.goalY, 2))

    # Calculate the angle distance between the robots heading and heading toward the goal
    skewX = self.goalX - self.odomX
    skewY = self.goalY - self.odomY
    dot = skewX * 1 + skewY * 0
    mag1 = math.sqrt(math.pow(skewX, 2) + math.pow(skewY, 2))
    mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))
    beta = math.acos(dot / (mag1 * mag2))
    if skewY < 0:
        if skewX < 0:
            beta = -beta
        else:
            beta = 0 - beta
    beta2 = (beta - angle)
    if beta2 > np.pi:
        beta2 = np.pi - beta2
        beta2 = -np.pi - beta2
    if beta2 < -np.pi:
        beta2 = -np.pi - beta2
        beta2 = np.pi - beta2

    # Publish the robot action
    vel_cmd = Twist()
    vel_cmd.linear.x = act[0]
    vel_cmd.angular.z = act[1]
    self.vel_pub.publish(vel_cmd)

    # Publish visual data in Rviz
    markerArray = MarkerArray()
    marker = Marker()
    marker.header.frame_id = "world"
    marker.type = marker.CYLINDER
    marker.action = marker.ADD
    marker.scale.x = 0.1
    marker.scale.y = 0.1
    marker.scale.z = 0.01
    marker.color.a = 1.0
    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.pose.orientation.w = 1.0
    marker.pose.position.x = self.goalX
    marker.pose.position.y = self.goalY
    marker.pose.position.z = 0

    markerArray.markers.append(marker)

    self.publisher.publish(markerArray)

    markerArray2 = MarkerArray()
    marker2 = Marker()
    marker2.header.frame_id = "world"
    marker2.type = marker.CUBE
    marker2.action = marker.ADD
    marker2.scale.x = abs(act[0])
    marker2.scale.y = 0.1
    marker2.scale.z = 0.01
    marker2.color.a = 1.0
    marker2.color.r = 1.0
    marker2.color.g = 0.0
    marker2.color.b = 0.0
    marker2.pose.orientation.w = 1.0
    marker2.pose.position.x = 5
    marker2.pose.position.y = 0
    marker2.pose.position.z = 0

    markerArray2.markers.append(marker2)
    self.publisher2.publish(markerArray2)

    markerArray3 = MarkerArray()
    marker3 = Marker()
    marker3.header.frame_id = "world"
    marker3.type = marker.CUBE
    marker3.action = marker.ADD
    marker3.scale.x = abs(act[1])
    marker3.scale.y = 0.1
    marker3.scale.z = 0.01
    marker3.color.a = 1.0
    marker3.color.r = 1.0
    marker3.color.g = 0.0
    marker3.color.b = 0.0
    marker3.pose.orientation.w = 1.0
    marker3.pose.position.x = 5
    marker3.pose.position.y = 0.2
    marker3.pose.position.z = 0

    markerArray3.markers.append(marker3)
    self.publisher3.publish(markerArray3)

    markerArray4 = MarkerArray()
    marker4 = Marker()
    marker4.header.frame_id = "world"
    marker4.type = marker.CUBE
    marker4.action = marker.ADD
    marker4.scale.x = 0.1  # abs(act2)
    marker4.scale.y = 0.1
    marker4.scale.z = 0.01
    marker4.color.a = 1.0
    marker4.color.r = 1.0
    marker4.color.g = 0.0
    marker4.color.b = 0.0
    marker4.pose.orientation.w = 1.0
    marker4.pose.position.x = 5
    marker4.pose.position.y = 0.4
    marker4.pose.position.z = 0

    markerArray4.markers.append(marker4)
    self.publisher4.publish(markerArray4)

    '''Bunch of different ways to generate the reward'''

    # reward = act[0]*0.7-abs(act[1])
    # r1 = 1 - 2 * math.sqrt(abs(beta2 / np.pi))
    # r2 = self.distOld - Dist
    r3 = lambda x: 1 - x if x < 1 else 0.0
    # rl = 0
    # for r in range(len(laser_state[0])):
    #    rl += r3(laser_state[0][r])
    # reward = 0.8 * r1 + 30 * r2 + act[0]/2 - abs(act[1])/2 - r3(min(laser_state[0]))/2
    reward = act[0] / 2 - abs(act[1]) / 2 - r3(min(laser_state[0])) / 2
    # reward = 30 * r2 + act[0] / 2 - abs(act[1]) / 2  # - r3(min(laser_state[0]))/2
    # reward = 0.8 * r1 + 30 * r2

    self.distOld = Dist

    # Detect if the goal has been reached and give a large positive reward
    if Dist < 0.6:
        target = True
        done = True
        self.distOld = math.sqrt(math.pow(self.odomX - self.goalX, 2) + math.pow(self.odomY - self.goalY, 2))
        reward = 80

    # Detect if ta collision has happened and give a large negative reward
    if col:
        reward = -100

    toGoal = [Dist, beta2, act[0], act[1]]
    state = np.append(laser_state, toGoal)
    return state, reward, done, target

def reset(self):

    # Resets the state of the environment and returns an initial observation.
    rospy.wait_for_service('/gazebo/reset_world')
    try:
        self.reset_proxy()

    except rospy.ServiceException as e:
        print("/gazebo/reset_simulation service call failed")

    angle = np.random.uniform(-np.pi, np.pi)
    quaternion = Quaternion.from_euler(0., 0., angle)
    object_state = self.set_self_state

    x = 0
    y = 0
    chk = False
    while not chk:
        x = np.random.uniform(-4.5, 4.5)
        y = np.random.uniform(-4.5, 4.5)
        chk = check_pos(x, y)
    object_state.pose.position.x = x
    object_state.pose.position.y = y
    # object_state.pose.position.z = 0.
    object_state.pose.orientation.x = quaternion.x
    object_state.pose.orientation.y = quaternion.y
    object_state.pose.orientation.z = quaternion.z
    object_state.pose.orientation.w = quaternion.w
    self.set_state.publish(object_state)

    self.odomX = object_state.pose.position.x
    self.odomY = object_state.pose.position.y

    self.change_goal()
    self.random_box()
    self.distOld = math.sqrt(math.pow(self.odomX - self.goalX, 2) + math.pow(self.odomY - self.goalY, 2))

    data = None
    rospy.wait_for_service('/gazebo/unpause_physics')
    try:
        self.unpause()
    except (rospy.ServiceException) as e:
        print("/gazebo/unpause_physics service call failed")
    while data is None:
        try:
            # data = rospy.wait_for_message('/r1/front_laser/scan', LaserScan, timeout=0.5)
            data = rospy.wait_for_message('/scan', LaserScan, timeout=0.5)
        except:
            pass
    laser_state = np.array(data.ranges[:])
    laser_state[laser_state == inf] = 10
    laser_state = binning(0, laser_state, 20)

    rospy.wait_for_service('/gazebo/pause_physics')
    try:
        self.pause()
    except (rospy.ServiceException) as e:
        print("/gazebo/pause_physics service call failed")

    Dist = math.sqrt(math.pow(self.odomX - self.goalX, 2) + math.pow(self.odomY - self.goalY, 2))

    skewX = self.goalX - self.odomX
    skewY = self.goalY - self.odomY

    dot = skewX * 1 + skewY * 0
    mag1 = math.sqrt(math.pow(skewX, 2) + math.pow(skewY, 2))
    mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))
    beta = math.acos(dot / (mag1 * mag2))

    if skewY < 0:
        if skewX < 0:
            beta = -beta
        else:
            beta = 0 - beta
    beta2 = (beta - angle)

    if beta2 > np.pi:
        beta2 = np.pi - beta2
        beta2 = -np.pi - beta2
    if beta2 < -np.pi:
        beta2 = -np.pi - beta2
        beta2 = np.pi - beta2

    toGoal = [Dist, beta2, 0.0, 0.0]
    state = np.append(laser_state, toGoal)
    return state

# Place a new goal and check if its lov\cation is not on one of the obstacles
def change_goal(self):
    if self.upper < 10:
        self.upper += 0.004
    if self.lower > -10:
        self.lower -= 0.004

    gOK = False

    while not gOK:
        self.goalX = self.odomX + random.uniform(self.upper, self.lower)
        self.goalY = self.odomY + random.uniform(self.upper, self.lower)
        gOK = check_pos(self.goalX, self.goalY)

# Randomly change the location of the boxes in the environment on each reset to randomize the training environment
def random_box(self):
    for i in range(4):
        name = 'cardboard_box_' + str(i)

        x = 0
        y = 0
        chk = False
        while not chk:
            x = np.random.uniform(-6, 6)
            y = np.random.uniform(-6, 6)
            chk = check_pos(x, y)
            d1 = math.sqrt((x - self.odomX) ** 2 + (y - self.odomY) ** 2)
            d2 = math.sqrt((x - self.goalX) ** 2 + (y - self.goalY) ** 2)
            if d1 < 1.5 or d2 < 1.5:
                chk = False
        box_state = ModelState()
        box_state.model_name = name
        box_state.pose.position.x = x
        box_state.pose.position.y = y
        box_state.pose.position.z = 0.
        box_state.pose.orientation.x = 0.0
        box_state.pose.orientation.y = 0.0
        box_state.pose.orientation.z = 0.0
        box_state.pose.orientation.w = 1.0
        self.set_state.publish(box_state)

[reiniscimurs]

Hi,

Melodic version of this repo is deprecated and is not supported from my side anymore. As such it is only provided "as is" and any changes you make should be up to you. However, the code itself should not impact your frame drift. Changing the collision distance would not change it either.

Most likely, you need to check your robot implementation and how you are setting up the transforms between different frames. But this is something you should be doing on your side as it is an unfamiliar issue and implementation for me. I do not see any obvious issues here, so I cannot help you with advice here either. Hope you manage to solve the issue.

[peipei518]

Thank you for your reply. The problem I am currently facing is that the collision reset environment can be achieved, but when the car reaches the target point, the environment will not be reset. Is this caused by the deviation of my odom or other factors? Can you give me some suggestions? Thank you!

[reiniscimurs]

Most likely the target point is actually not arrived at. Since you are displaying in a different frame than odom, it might appear in rviz that the goal is reached. But the reached goal trigger is the relevant distance between the goal and the robot in odom frame. You should print out the distance values to confirm that distance actually never goes below the threshold of triggering a reached goal.

I would guess that something is misaligned with your frames. Unfortunately, I cannot help you there as I have no experience in implementing the robot you are trying to use.

[peipei518]

Thank you for your reply. With your suggestion, I attempted to print the distance value from the target point and found that, as you mentioned, when I selected World, it was displayed as arrived but not actually arrived. When I selected Odom, although the radar data would shift with the car's movement, it could still reach the target point normally and reset the environment. I tried debugging my code for a day, but I couldn't find the correct direction. I tried changingtrueto false, but the radar data was still offset when choosing odom. I also tried changing to false and added the tf transformation from world to odom from the launch file. As a result, the radar data also encountered the above problem when choosing world, What I am currently wondering is why radar data is only normal when choosing World. Is it defined in my xacro file? I will provide you with the xacro file of my car model, and if possible, I would like to receive some suggestions from you. This is my plugin file <?xml version="1.0"?>

0.3 0.3 0.3 0.3 1.5 1.5 1.5 1.5 1.5 1.5 1.5 1.5 Debug cmd_vel odom odom world base_link world false true true false false 0.80 0.25 0.0 100.0 10.0 joint_left_front joint_left_back joint_right_front joint_right_back true true 20 true imu imu imu_link 10 0.0 0 0 0 0 0 0 imu_link true false 500 1.0 -1.0 1.0 160 1.0 -0.1 0.1 0.05 160.0 0.00 true 10 points2 lider_1_link true 10 This is my vehicle's xacro file    

[peipei518]

[reiniscimurs]

Sorry, this is outside the scope of this repo. I do not know what your full robot setup is like, I cannot help you with that. All I can say is that I do not understand what is this world frame is and why base_link has a transform aiming towards it. This seems strange. Most likely, your transforms are set up wrong. But as I said, this is not something I can help you with.

[peipei518]

Thank you very much for your help. I found that the problem was caused by the Gazebo driver plugin. I replaced the plugin and the problem was resolved