#!/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')
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()