ROS Basics Real Robot Project - Section 2

[nhnghiaworks]

Struggling in Section 2 of Ros Basics Real Robot Project. I’m implementing find_wall_server.py but when I rosrun it, nothing happen with the robot. Can someone correct me, I think it in terms of configuration.
This is my find_wall_server.py

#!/usr/bin/env python

import rospy
from rosject1.srv import FindWall, FindWallResponse
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

class WallFinder:
    def __init__(self):
        rospy.init_node('find_wall_server')
        self.publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.laser_subscriber = rospy.Subscriber('/scan', LaserScan, self.laser_callback)
        self.angular_velocity = 0.5
        self.linear_velocity = 0.2
        self.min_distance = 0.3
        self.target_angle = 270
        self.latest_laser_data = None

    def laser_callback(self, laser_data):
        self.latest_laser_data = laser_data

    def handle_find_wall(self, req):
        response = FindWallResponse()

        if self.latest_laser_data is None:
            rospy.logwarn("No laser data received yet.")
            return response

        shortest_ray_index = self.latest_laser_data.ranges.index(min(self.latest_laser_data.ranges))

        # Step 2: Rotate robot to face the wall
        while self.latest_laser_data.ranges[shortest_ray_index] > self.min_distance:
            self.rotate(self.angular_velocity)
        self.stop_movement()

        # Step 3: Move robot forward
        while self.latest_laser_data.ranges[shortest_ray_index] > self.min_distance:
            self.move_forward(self.linear_velocity)
        self.stop_movement()

        # Step 4: Rotate robot again
        while shortest_ray_index != self.target_angle:
            self.rotate(self.angular_velocity)
        self.stop_movement()

        response.wallfound = True
        return response

    def rotate(self, angular_velocity):
        twist = Twist()
        twist.angular.z = angular_velocity
        self.publisher.publish(twist)
        rospy.sleep(0.1)
    
    def move_forward(self, linear_velocity):
        twist = Twist()
        twist.linear.x = linear_velocity
        self.publisher.publish(twist)
        rospy.sleep(0.1)
    
    def stop_movement(self):
        twist = Twist()
        self.publisher.publish(twist)

if __name__ == '__main__':
    wall_finder = WallFinder()
    service = rospy.Service('find_wall', FindWall, wall_finder.handle_find_wall)
    rospy.spin()

This is my CMakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(rosject1)

find_package(catkin REQUIRED COMPONENTS
  rospy
  sensor_msgs
  std_msgs
  message_generation
)

add_service_files(
  FILES
  FindWall.srv
)

generate_messages(
  DEPENDENCIES
  sensor_msgs std_msgs
)

catkin_package(
  CATKIN_DEPENDS rospy std_msgs sensor_msgs message_runtime
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

catkin_install_python(
  PROGRAMS scripts/find_wall_server.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

And package.xml

<?xml version="1.0"?>
<package format="2">
  <name>rosject1</name>
  <version>0.0.0</version>
  <description>The rosject1 package</description>

  <maintainer email="user@todo.todo">user</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>rospy</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <build_depend>message_runtime</build_depend>
  <exec_depend>message_runtime</exec_depend>

  <export>
    <!-- You can add other export-related tags here if needed -->
  </export>
</package>

The rosject1.launch

<launch>
  <!-- Launch the wall following node -->
  <node name="wall_following_node" pkg="rosject1" type="wall_following_node.py" output="screen"/>

  <!-- Launch the service server node -->
  <node name="find_wall_server" pkg="rosject1" type="find_wall_server.py" output="screen"/>
</launch>

I would like to have a way to restart my workspace as well, during implementing I delete ~/catkin_ws/build to catkin build the package again, so it unstable.

Thanks for your help.

[girishkumar.kannan]

Hi @nhnghiaworks ,

Welcome to this Community!

There are two things that you are not considering - the rotational inertia and the precision of data.

When you read the laser scanner WHILE you are rotating at an angular velocity of 0.5 rads/sec, you are trying to get the scan reading spinning so fast. Naturally, the robot cannot find the minimum value.

Besides the above reason, you cannot exceed 0.19 m/s speed for linear and 0.49 rads/sec speed for angular. It is written in the instructions at the bottom. So real robot will not even move, when you have such high values. I would tell you to not exceed 0.15 m/s and 0.45 rads/rec as a rule.

Now regarding precision of data, scanner usually reports scan values in as double - with 15 decimal places of precision - you will NEVER match that value when you scan. So round off every value to 3 decimal places. Remember, the distance is in meters, so 1.000 is 1 meter, and 0.001 means 1 millimeter. Any precision beyond 3 decimal places is useless. So even if you use float with 6 decimal place accuracy, you will still not make the robot converge at a solution.

Lastly, don’t do scanning while moving (angular or linear). It will not give you good results. Perform a move-> stop->scan procedure instead of move->scan procedure. This will make the scans more reliable. Scanning while moving is not suitable for small distances.

Implementing these fixes will get your program to work correctly.

Regards,
Girish