Adding Lidar seonsor and storing data from Lidar and front camera

[am-amani]

Hi Reinis,

Thank you for sharing this project with everyone. I have two questions. Is it possible to help me?

1. I would like to add a Lidar sensor because my mobile robot has a lidar sensor. Could you please guide me on how I can do so?
2. Then I would like to store lidar data and camera data on my laptop, when I am runinng on the trained model. Could you please help me as well?

[reiniscimurs]

Hi,

1. We already use a lidar sensor here. It is a Velodyne Puck 3D lidar. If you wish to change the sensor to something else you can do so and update the code accordingly and adding it to the robot here: https://github.com/reiniscimurs/DRL-robot-navigation/blob/main/catkin_ws/src/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro If you mean you want to use 2D lidar, then there is already a 2D hokuyo lidar added to the robot. You need to create a subscriber for it in the code and update the code to use it for laser state. You can see amethod of how to do that in a different repo here: https://github.com/reiniscimurs/GDAE/blob/main/Code/GDAM_env.py#L144

2. You can use rosbag or other saving methods. That depends entirely how you want to go about it and what you want to do with the data, what format you want to save it in and what form the lidar and image data should take. On its own it is too broad of a question to give you any hints at this time.

[am-amani]

Thank you for getting back to me. I noticed that you have already implemented a replay_buffer.py. Can you guide me on storing the data within the replay buffer? Should I create a node and subscribe to the related topics?

[am-amani]

I have added some modifications in test_velodyne_td3.py for reading the camera and sensor data and adding to my replay buffer, but I got an error that I don't understand it.

import time
import numpy as np
import torch
import torch.nn as nn
import torch.nn.functional as F
import rospy
from sensor_msgs.msg import Image, LaserScan
from cv_bridge import CvBridge

from velodyne_env import GazeboEnv

# Actor network
class Actor(nn.Module):
    def __init__(self, state_dim, action_dim):
        super(Actor, self).__init__()
        self.layer_1 = nn.Linear(state_dim, 800)
        self.layer_2 = nn.Linear(800, 600)
        self.layer_3 = nn.Linear(600, action_dim)
        self.tanh = nn.Tanh()

    def forward(self, s):
        s = F.relu(self.layer_1(s))
        s = F.relu(self.layer_2(s))
        a = self.tanh(self.layer_3(s))
        return a

# TD3 network
class TD3(object):
    def __init__(self, state_dim, action_dim):
        self.actor = Actor(state_dim, action_dim).to(device)

    def get_action(self, state):
        state = torch.Tensor(state.reshape(1, -1)).to(device)
        return self.actor(state).cpu().data.numpy().flatten()

    def load(self, filename, directory):
        self.actor.load_state_dict(torch.load("%s/%s_actor.pth" % (directory, filename)))

# Replay Buffer
class ReplayBufferROSTopics(object):
    def __init__(self, state_dim, action_dim, lidar_size, image_shape, max_size=int(1e6)):
        self.max_size = max_size
        self.ptr = 0
        self.size = 0
        self.state = np.zeros((max_size, state_dim))
        self.action = np.zeros((max_size, action_dim))
        self.reward = np.zeros((max_size, 1))
        self.not_done = np.zeros((max_size, 1))
        self.lidar_scan = np.zeros((max_size, lidar_size))
        self.camera_image = np.zeros((max_size, *image_shape))
        self.next_state = np.zeros((max_size, state_dim))
        self.next_lidar_scan = np.zeros((max_size, lidar_size))
        self.next_camera_image = np.zeros((max_size, *image_shape))

    def add(self, state, action, reward, done, lidar_scan, camera_image, next_state, next_lidar_scan, next_camera_image):
        self.state[self.ptr] = state
        self.action[self.ptr] = action
        self.reward[self.ptr] = reward
        self.not_done[self.ptr] = 1 - done
        self.lidar_scan[self.ptr] = lidar_scan
        self.camera_image[self.ptr] = camera_image
        self.next_state[self.ptr] = next_state
        self.next_lidar_scan[self.ptr] = next_lidar_scan
        self.next_camera_image[self.ptr] = next_camera_image
        self.ptr = (self.ptr + 1) % self.max_size
        self.size = min(self.size + 1, self.max_size)

    def sample(self, batch_size):
        ind = np.random.randint(0, self.size, size=batch_size)
        return (
            torch.FloatTensor(self.state[ind]).to(device),
            torch.FloatTensor(self.action[ind]).to(device),
            torch.FloatTensor(self.reward[ind]).to(device),
            torch.FloatTensor(self.not_done[ind]).to(device),
            torch.FloatTensor(self.lidar_scan[ind]).to(device),
            torch.FloatTensor(self.camera_image[ind]).to(device),
            torch.FloatTensor(self.next_state[ind]).to(device),
            torch.FloatTensor(self.next_lidar_scan[ind]).to(device),
            torch.FloatTensor(self.next_camera_image[ind]).to(device)
        )

# Initialize ROS node and CvBridge
rospy.init_node('data_collector_node')
bridge = CvBridge()

# Global variables to store latest data from ROS topics
latest_camera_image = None
latest_lidar_scan = None

def image_callback(msg):
    global latest_camera_image
    try:
        latest_camera_image = bridge.imgmsg_to_cv2(msg, "bgr8")
    except Exception as e:
        rospy.logerr("Error processing image data: %s", e)

def scan_callback(msg):
    global latest_lidar_scan
    latest_lidar_scan = msg.ranges

image_sub = rospy.Subscriber('/r1/front_camera/image_raw', Image, image_callback)
scan_sub = rospy.Subscriber('/r1/front_laser/scan', LaserScan, scan_callback)

# Main part of your existing project code
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
seed = 0
max_ep = 500
file_name = "TD3_velodyne"

environment_dim = 20
robot_dim = 4
env = GazeboEnv("multi_robot_scenario.launch", environment_dim)
time.sleep(5)
torch.manual_seed(seed)
np.random.seed(seed)
state_dim = environment_dim + robot_dim
action_dim = 2

# Initialize Replay Buffer
# Assuming lidar scan size and camera image shape - adjust these as per your requirements
lidar_scan_size = 360
camera_image_shape = (64, 64, 3)
replay_buffer = ReplayBufferROSTopics(state_dim, action_dim, lidar_scan_size, camera_image_shape)

# Load network
network = TD3(state_dim, action_dim)
try:
    network.load(file_name, "./pytorch_models")
except:
    raise ValueError("Could not load the stored model parameters")

# Main loop
done = False
episode_timesteps = 0
state = env.reset()

while True:
    action = network.get_action(np.array(state))
    a_in = [(action[0] + 1) / 2, action[1]]
    next_state, reward, done, target = env.step(a_in)
    done = 1 if episode_timesteps + 1 == max_ep else int(done)

    # Assuming latest_camera_image and latest_lidar_scan are updated by ROS subscribers
    next_lidar_scan = latest_lidar_scan  # Define next_lidar_scan variable
    next_camera_image = latest_camera_image  # Define next_camera_image variable
    replay_buffer.add(state, action, reward, done, latest_lidar_scan, latest_camera_image, next_state, next_lidar_scan, next_camera_image)

    if done:
        state = env.reset()
        done = False
        episode_timesteps = 0
    else:
        state = next_state
        episode_timesteps += 1

---

Here the error that I get: amirmahdi@amirmahdi:~/DRL-robot-navigation/TD3$ python3 test_velodyne_td3.py Unable to register with master node [http://localhost:11311]: master may not be running yet. Will keep trying.

[reiniscimurs]

Thank you for getting back to me. I noticed that you have already implemented a replay_buffer.py. Can you guide me on storing the data within the replay buffer? Should I create a node and subscribe to the related topics?

Replay buffer is just a data structure built upon pythons double edged queue. You do not require any nodes or topics to store data there. You store any data that you want there just as you would in a deque.

Here the error that I get: amirmahdi@amirmahdi:~/DRL-robot-navigation/TD3$ python3 test_velodyne_td3.py Unable to register with master node [http://localhost:11311]: master may not be running yet. Will keep trying.

This error is not related to the code changes. In any case, this file does not really deal with ROS directly. I suspect you did not close your previous run properly. After exiting a run you need to stop all the background processes with killall -9 rosout roslaunch rosmaster gzserver nodelet robot_state_publisher gzclient python python3 This is explained in the tutorial: https://medium.com/@reinis_86651/deep-reinforcement-learning-in-mobile-robot-navigation-tutorial-part1-installation-d62715722303

[am-amani]

Do you mean that it is not required? I created this node and topics because I needed to read the lidar and camera data. How can read these data without subscribing to that topic? As I think now, a node may not be required, but a subscriber is required, isn't it?

[reiniscimurs]

Do you mean that it is not required? I created this node and topics because I needed to read the lidar and camera data. How can read these data without subscribing to that topic? As I think now, a node may not be required, but a subscriber is required, isn't it?

I think you are asking a different question then. Replay buffer is just a data structure where we store the experience tuple. This tuple then has state and next_state. Whatever is given in these states will get saved in the replay buffer already. What you want is to get new types of sensor and save that into the state. For this you will need a subscriber (the RGB camera images are published already) and then restructure the state variable so that it can store this extra information. You will then also require to update the training of the model to be able to extract this data and the model and network structure to allow learning from such data. While it is not too difficult from programming standpoint (depending on your skill level), it might take some finessing. As such, the question is quite broad and I can only answer some specific questions.

[am-amani]

No, I actually mean that I wanted to store the lidar data and camera data when the test file is running. I could solve my problem. Thank you for giving me the hints :)