Difference between revisions of "HamsterPython"
| Line 40: | Line 40: | ||
rospy.spin() | rospy.spin() | ||
| + | </syntaxhighlight> | ||
| + | |||
| + | |||
| + | <syntaxhighlight lang="python" line='line'> | ||
| + | |||
| + | #!/usr/bin/env python | ||
| + | import rospy | ||
| + | import math | ||
| + | from sensor_msgs.msg import Imu | ||
| + | from tf.transformations import euler_from_quaternion, quaternion_from_euler | ||
| + | |||
| + | def calculate_robot_angle(orientation): | ||
| + | quaternion = ( | ||
| + | orientation.x, | ||
| + | orientation.y, | ||
| + | orientation.z, | ||
| + | orientation.w) | ||
| + | euler = euler_from_quaternion(quaternion) | ||
| + | roll = euler[0] | ||
| + | pitch = euler[1] | ||
| + | yaw = euler[2] | ||
| + | return math.degrees(yaw) | ||
| + | |||
| + | prev_angle = 0 | ||
| + | def callback(msg): | ||
| + | global prev_angle | ||
| + | curr_angle = calculate_robot_angle(msg.orientation) | ||
| + | if curr_angle > prev_angle : | ||
| + | print "TURINING LEFT" | ||
| + | else: | ||
| + | print "TURINIGN RIGHT" | ||
| + | prev_angle = curr_angle | ||
| + | |||
| + | |||
| + | rospy.init_node('imu_values') | ||
| + | last_angle = 0 | ||
| + | |||
| + | #Put your Hamster number instead of agent<number> | ||
| + | #The agent number is printed on the Hamster cover | ||
| + | sub = rospy.Subscriber('/agent14/imu', Imu, callback) | ||
| + | |||
| + | |||
| + | rospy.spin() | ||
</syntaxhighlight> | </syntaxhighlight> | ||
Revision as of 14:35, 19 March 2019
Python setup
- Note you are on the HamsterNet
<syntaxhighlight lang="bash" line='line'>
$ export ROS_IP=<your IP>
</syntaxhighlight>
<syntaxhighlight lang="bash" line='line'>
$ export ROS_MASTER_URI=http://10.0.2.<HAMSTER AGENT Number>:11311
</syntaxhighlight>
Python Example 1
- Receive laser scan from robot to decide if free
<syntaxhighlight lang="python" line='line'>
- !/usr/bin/env python
import rospy from sensor_msgs.msg import LaserScan
def is_free(ranges ,start_index, end_index ,min_distance): #sub array from angle to angle s_ranges = ranges[start_index:end_index] #filter the array b_ranges = filter(lambda x: x <= min_distance, s_ranges) if len(b_ranges) > 1 : print "BLOCKED" else : print "FREE"
def callback(msg):
is_free(msg.ranges,160,200,0.2)
rospy.init_node('scan_values')
- Put your Hamster number instead of agent<number>
- The agent number is printed on the Hamster cover
sub = rospy.Subscriber('/agent14/scan', LaserScan, callback)
rospy.spin()
</syntaxhighlight>
<syntaxhighlight lang="python" line='line'>
- !/usr/bin/env python
import rospy import math from sensor_msgs.msg import Imu from tf.transformations import euler_from_quaternion, quaternion_from_euler
def calculate_robot_angle(orientation): quaternion = ( orientation.x, orientation.y, orientation.z, orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] return math.degrees(yaw)
prev_angle = 0 def callback(msg): global prev_angle curr_angle = calculate_robot_angle(msg.orientation) if curr_angle > prev_angle : print "TURINING LEFT" else: print "TURINIGN RIGHT" prev_angle = curr_angle
rospy.init_node('imu_values')
last_angle = 0
- Put your Hamster number instead of agent<number>
- The agent number is printed on the Hamster cover
sub = rospy.Subscriber('/agent14/imu', Imu, callback)
rospy.spin()
</syntaxhighlight>
