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.