#!/usr/bin/env python
import rospy
from phantomx_gazebo.phantomx import PhantomX
if __name__ == '__main__':
rospy.init_node('follow_wall')
robot = PhantomX()
rospy.sleep(1)
while not rospy.is_shutdown():
z = robot.follow_wall()
robot.set_walk_velocity(0.6, 0, z)
rospy.sleep(0.1)
def turn_left():
robot.set_walk_velocity(0.7, 0, 0.5)
rospy.sleep(0.15)
def go_forward():
robot.set_walk_velocity(1, 0, 0)
rospy.sleep(0.15)
if __name__ == '__main__':
rospy.init_node('hexabot_exploration')
rospy.loginfo('Instantiating robot Client')
robot = PhantomX()
rospy.sleep(1)
rospy.loginfo('Cave Exploration Starting')
#print robot.lidar_ranges[180] #180=front, 270=left, 90=right
rate = rospy.Rate(25)
while not rospy.is_shutdown():
global regions_
regions_ = {
'left': min(min(robot.lidar_ranges[700:720]), 10),
'fleft': min(min(robot.lidar_ranges[380:700]), 10),
'front': min(min(robot.lidar_ranges[340:380]), 10),
'fright': min(min(robot.lidar_ranges[20:340]), 10),
'right': min(min(robot.lidar_ranges[0:20]), 10)
}
#!/usr/bin/env python
import rospy
from phantomx_gazebo.phantomx import PhantomX
if __name__ == '__main__':
rospy.init_node('walker_demo')
rospy.loginfo('Instantiating robot Client')
robot = PhantomX()
rospy.sleep(1)
rospy.loginfo('Walker Demo Starting')
robot.set_walk_velocity(0.2, 0, 0)
rospy.sleep(3)
robot.set_walk_velocity(1, 0, 0)
rospy.sleep(3)
robot.set_walk_velocity(0, 1, 0)
rospy.sleep(3)
robot.set_walk_velocity(0, -1, 0)
rospy.sleep(3)
robot.set_walk_velocity(-1, 0, 0)
rospy.sleep(3)
robot.set_walk_velocity(1, 1, 0)
rospy.sleep(5)
robot.set_walk_velocity(0, 0, 0)
rospy.loginfo('Walker Demo Finished')
#!/usr/bin/env python
import rospy
from phantomx_gazebo.phantomx import PhantomX
if __name__ == '__main__':
rospy.init_node('walker_demo')
rospy.loginfo('Instantiating robot Client')
robot = PhantomX()
rospy.sleep(1)
rospy.loginfo('Walker Demo Starting')
robot.set_walk_velocity(0.2, 0, 0)
rospy.sleep(3)
robot.set_walk_velocity(1, 0, 0)
rospy.sleep(3)
robot.set_walk_velocity(0, 1, 0)
rospy.sleep(3)
robot.set_walk_velocity(0, -1, 0)
rospy.sleep(3)
robot.set_walk_velocity(-1, 0, 0)
rospy.sleep(3)
robot.set_walk_velocity(1, 1, 0)
rospy.sleep(5)
robot.set_walk_velocity(0, 0, 0)
rospy.loginfo('Walker Demo Finished')
rospy.loginfo("Distancia: %f Angulo: %f", data.ranges[n], n)
if data.ranges[n] > 1.1:
rospy.loginfo("if")
robot.set_walk_velocity(0, 0, 0)
rospy.sleep(0.2)
robot.set_walk_velocity(-1, 0, 0)
rospy.sleep(1)
robot.set_walk_velocity(1, 0, 0.5)
rospy.sleep(2)
else:
rospy.loginfo("else")
robot.set_walk_velocity(1, 0, 0)
rospy.sleep(0.1)
n = n + 72
def listener():
rospy.Subscriber("/phantomx/laser/scan", LaserScan, callback)
rospy.spin()
if __name__ == "__main__":
rospy.init_node("walker_demo")
rospy.loginfo("Instanciando Phantomx")
robot = PhantomX()
rospy.sleep(1)
rospy.loginfo("Robot Listo!")
robot.set_walk_velocity(0, 0, 0)
listener()
rospy.loginfo("Robot Apagado!")
self.ranges = []
self.K = K
def _callback_scan(self, msg):
self.ranges = msg.ranges
def follow_wall(self):
d_mur = self.ranges[90]
d_consigne = 0.8
z = self.K*(d_mur - d_consigne)
if z > 1 :
z = 1
if z < -1 :
z = -1
return z
if __name__ == '__main__':
rospy.init_node('follow_wall')
robot = PhantomX()
follow = Follow_wall()
rospy.sleep(1)
while not rospy.is_shutdown():
z = follow.follow_wall()
robot.set_walk_velocity(0.5, 0, z)
print(z)
rospy.sleep(0.1)
#!/usr/bin/env python
# Publishes to the lidar so it moves at the correct speed
import rospy
from phantomx_gazebo.phantomx import PhantomX
from std_msgs.msg import Float64
if __name__ == '__main__':
rospy.init_node('lidar_sweep_node')
rospy.loginfo('Instantiating robot Client')
robot = PhantomX()
rate = rospy.Rate(50) #hz
angle = 0
speed = 0.005
while not rospy.is_shutdown():
# rospy.loginfo(angle)
robot.set_lidar_theta(angle)
if((angle)<-0.01 or angle>1.2):
speed = speed*-1
angle=angle+speed
rate.sleep()
