It’s hard to say without seeing your full package structure. Double and tripple check that the names are exactly correct and that all files are executable.
I finally got it running with no errors thank you, however, the Sphero moved once, it hits the wall and then it stops…, (I’m just testing codes provided in the project) , the Turtlebot project worked well, it actually moves around the maze, but Sphero doesn’'t…
Any advises please?
Thank you for your help,
Kind Regards,
@wai.mak
Also, there are errors as well when I do ctrl+c
File “/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/timer.py”, line 166, in sleep
raise rospy.exceptions.ROSInterruptException(“ROS shutdown request”)
rospy.exceptions.ROSInterruptException: ROS shutdown request
[ERROR] [1589036699.948718, 5924.491000]: Exception in your execute callback: ROS shutdown request
Traceback (most recent call last):
File “/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/simple_action_server.py”, line 289, in executeLoop
self.execute_callback(goal)
File “/home/user/catkin_ws/src/my_sphero_main/src/rec_odom_action_server.py”, line 57, in goal_callback
rate.sleep()
File “/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/timer.py”, line 103, in sleep
sleep(self._remaining(curr_time))
File “/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/timer.py”, line 166, in sleep
raise rospy.exceptions.ROSInterruptException(“ROS shutdown request”)
ROSInterruptException: ROS shutdown request
shutting down processing monitor…
… shutting down processing monitor complete
done
user:~/catkin_ws$
Please advise,
Thank you for your help,
Kind Regards,
@wai.mak
As by your code LOG messages, it looks like it is just moving forward. You have to detect a collision and then do something like turn, or back up and turn. To detect collisions, use the IMU topic. IMU stands for Inertial Measuring Unit.
This error is normal. You are manually breaking the code. This can be useful when your code seems to be stuck in a loop and you don’t know where. The error tells you, where it interrupted the code.
Thank you very much for your quick reply,
the main code provided only imported odometry (below)
so does that mean I need to import IMU topic as well to the main program and write code myself to utilize detect collision?
import rospy
import actionlib
from std_srvs.srv import Trigger, TriggerRequest
from exam_action_rec_odom.msg import record_odomGoal, record_odomFeedback, record_odomResult, record_odomAction
from cmd_vel_publisher import CmdVelPub
from odometry_analysis import OdometryAnalysis
from odometry_analysis import check_if_out_maze
Thank you for your help,
I look forward to your reply,
Kind Regards,
@wai.mak
I really recommend trying to solve this project completely on your own. It might take a bit, but you will really start to understand things, when you solved them yourself. Use things like rostopic list, rosnode list, rostopic info, rosnode info, rviz to understand the robot and how it operates
Then try and follow the rough steps outlined in the notebook.
- Step 1: Read and Write Topics (Dedicate 2 hours)
- Step 2: Use topics through Services (Dedicate 3 hours)
- Step 3: Use topics through Actions (Dedicate 4 hours)
- Step 4: Create a main program to manage everything (Dedicate 1 hour)
- EXTRA Step: How to use python modules from different packages (Not required, included here for information purposes only)
Get creative, explore, dig, come up with solutions, test them. There are many ways to solve this project. But you will have to figure some things out:
- how to move the robot
- how to detect collisions
- how to know the robot is out of the maze
- a logic that makes decisions on how to move it
I found that I did use the IMU topic and also tried to use some logic to move back and forth when a crash to the wall is detected,
However, I found the problem which I may have is with the IMU_topic_subscriber.py (in my my_sphero_main package/src folder)
Line 100: return self.convert_to_dict(message) in the method four_sector_detection(self): , it however places all the four sides to false which in fact front should be true because it has a crash. So not sure what went wrong in this four_vector_detection() method …, please see screenshot , I have added a line print(detection_dict) at the end of the four_vector_detection() method to display the dictionary telling in which direction there is a detection of a crash, (it displays all directions as false…) any reason why? or have I interpret it wrongly?
I know I’m supposed to figure out everything myself, but I’m really stuck and I’m wondering isn’t the supplied solution code of the imu_topic_subscriber.py be workable?
I look forward to your reply,
Thanks for your help,
Kind Regards,
@wai.mak
Hey @wai.mak,
please paste the code of your direction server here. Hard to guess what is wrong without seeing the code. Paste the code, mark it and then click that ‘</>’ symbol to format it as code
here is the code of the direction server,
#! /usr/bin/env python
import rospy
from std_srvs.srv import Trigger, TriggerResponse
from imu_topic_susbcriber import ImuTopicReader
import time
class CrashDirectionService(object):
def __init__(self, srv_name='/crash_direction_service'):
self._srv_name = srv_name
self._imu_reader_object = ImuTopicReader()
self.detection_dict = {"front":False, "left":False, "right":False, "back":False}
self._my_service = rospy.Service(self._srv_name, Trigger , self.srv_callback)
rospy.logdebug("DIRECTION ==>")
def srv_callback(self, request):
self.detection_dict = self._imu_reader_object.four_sector_detection()
message = self.direction_to_move()
rospy.logdebug("[LEFT="+str(self.detection_dict["left"])+", FRONT="+str(self.detection_dict["front"])+", RIGHT="+str(self.detection_dict["right"])+"]"+", BACK="+str(self.detection_dict["back"])+"]")
rospy.logdebug("DIRECTION ==>"+message)
response = TriggerResponse()
"""
---
bool success # indicate if crashed
string message # Direction
"""
response.success = self.has_crashed()
print(response.success)
response.message = message
return response
def has_crashed(self):
for key, value in self.detection_dict.iteritems():
if value:
return True
return False
def direction_to_move(self):
if not self.detection_dict["front"]:
message = "forwards"
else:
if not self.detection_dict["back"]:
message = "backwards"
else:
if not self.detection_dict["left"]:
message = "left"
else:
if not self.detection_dict["right"]:
message = "right"
else:
message = "un_stuck"
return message
if __name__ == "__main__":
rospy.init_node('crash_direction_service_server', log_level=rospy.INFO)
dir_serv_object = CrashDirectionService()
rospy.spin() # mantain the service open.
thanks for your help,
Kind Regards,
@wai.mak
Have you implemented the
imu_topic_susbcriber
This seems to be where the crash detection actually happens. (see this line:)
self.detection_dict = self._imu_reader_object.four_sector_detection()
Yes I notice that,
and the query is why the line response.success = self.has_crashed() ’ always returns all false to the variable response.success? This is the part I really dont get…
Thank you,
Kind Regards,
Roger
It’s all in the imu_reader:
self.detection_dict = self._imu_reader_object.four_sector_detection()
So only if the imu_subscriber detects a crash, this function returns True
def has_crashed(self):
for key, value in self.detection_dict.iteritems():
if value:
return True
return False
Yes thats right,
but when the Sphero hits the wall, it seems has_crashed always returned just ‘false’ and never had returned ‘true’ when the Sphero has crashed,
this is the part where I dont understand, this is the supplied code in the Sphero project, really dont know why it doesnt work as it should be intended to…can you please explain?
Thank you for your help,
Kind Regards,
@wai.mak
Can you post your imu_subscriber code?
Hi Simon,
Here is the imu_topic_subscriber.py code:
#! /usr/bin/env python
import rospy
from sensor_msgs.msg import Imu
class ImuTopicReader(object):
def __init__(self, topic_name = '/sphero/imu/data3'):
self._topic_name = topic_name
self._sub = rospy.Subscriber(self._topic_name, Imu, self.topic_callback)
self._imudata = Imu()
self._threshhold = 7.00
def topic_callback(self, msg):
self._imudata = msg
rospy.logdebug(self._imudata)
def get_imudata(self):
"""
Returns the newest imu data
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance
"""
return self._imudata
def four_sector_detection(self):
"""
Detects in which four directions there is an obstacle that made the robot crash
Based on the imu data
Axis:
^y
|
zO-->x
"""
x_accel = self._imudata.linear_acceleration.x
y_accel = self._imudata.linear_acceleration.y
z_accel = self._imudata.linear_acceleration.z
axis_list = [x_accel, y_accel, z_accel]
max_axis_index = axis_list.index(max(axis_list))
positive = axis_list[max_axis_index] >= 0
significative_value = abs(axis_list[max_axis_index]) > self._threshhold
if significative_value:
if max_axis_index == 0:
# Winner is in the x axis, therefore its a side crash left/right
rospy.logwarn("[X="+str(x_accel))
rospy.loginfo("Y="+str(y_accel)+", Z="+str(z_accel)+"]")
if positive:
message = "right"
else:
message = "left"
elif max_axis_index == 1:
# Winner is the Y axis, therefore its a forn/back crash
rospy.logwarn("[Y="+str(y_accel))
rospy.loginfo("X="+str(x_accel)+", Z="+str(z_accel)+"]")
if positive:
message = "front"
else:
message = "back"
elif max_axis_index == 2:
# Z Axixs is the winner, therefore its a crash that made it jump
rospy.logwarn("[Z="+str(z_accel))
rospy.loginfo("X="+str(x_accel)+", Y="+str(y_accel)+"]")
if positive:
message = "up"
else:
message = "down"
else:
message = "unknown_direction"
else:
rospy.loginfo("X="+str(x_accel)+"Y="+str(y_accel)+", Z="+str(z_accel)+"]")
message = "nothing"
return self.convert_to_dict(message)
def convert_to_dict(self, message):
"""
Converts the fiven message to a dictionary telling in which direction there is a detection
"""
detect_dict = {}
# We consider that when there is a big Z axis component there has been a very big front crash
detection_dict = {"front":(message=="front" or message=="up" or message=="down"),
"left":message=="left",
"right":message=="right",
"back":message=="back"}
print(detection_dict)
return detection_dict
if __name__ == "__main__":
rospy.init_node('imu_topic_subscriber', log_level=rospy.INFO)
imu_reader_object = ImuTopicReader()
rospy.loginfo(imu_reader_object.get_imudata())
rate = rospy.Rate(0.5)
ctrl_c = False
def shutdownhook():
# works better than the rospy.is_shut_down()
global ctrl_c
print "shutdown time!"
ctrl_c = True
rospy.on_shutdown(shutdownhook)
while not ctrl_c:
data = imu_reader_object.get_imudata()
rospy.loginfo(data)
rate.sleep()
Thank you for your help,
Kind Regards,
@wai.mak
Perhaps
self._threshhold = 7.00
is too high. Try lowering that number. Test the if the detection works in general by adding print or log messages in the code.
from imu_topic_susbcriber import ImuTopicReader
ImportError: No module named imu_topic_susbcriber
You should really fix the typo here, it took me while to figure out whats wrong: it is spelled SUB-scriber, not susb-criber…