multi_robot_control.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; 
00004 import rospy
00005 from sensor_msgs.msg import Joy 
00006 from geometry_msgs.msg import Twist
00007 from ric_robot.msg import ric_pan_tilt
00008 
00009 def joy_callback(data):
00010     global msg
00011     global msg_pt
00012     global max_vel
00013     global max_rot
00014     global msg_pt
00015     global max_pan
00016     global max_tilt
00017     global pan_vel
00018     global tilt_vel
00019     global pan_dir
00020     global tilt_dir
00021     global scroll_btn
00022     global current_robot
00023     global num_robots
00024     global joy_for
00025     global joy_rot
00026     global joy_pan
00027     global joy_tilt
00028     global joy_scroll_btn
00029     global joy_slow_btn
00030     global slow
00031     global slow_btn
00032     global joy_center_btn
00033     global center_btn
00034     global center
00035     msg.linear.x=data.axes[joy_for]*max_vel*slow
00036     msg.angular.z=data.axes[joy_rot]*max_rot*slow
00037     if (data.axes[joy_pan]>0.5 and msg_pt.pan_angle<max_pan):
00038       pan_dir=1
00039     elif (data.axes[joy_pan]<-0.5 and msg_pt.pan_angle>-max_pan):
00040       pan_dir=-1
00041     else:
00042       pan_dir=0
00043     if (data.axes[joy_tilt]>0.5 and msg_pt.tilt_angle<max_tilt):
00044       tilt_dir=1
00045     elif (data.axes[joy_tilt]<-0.5 and msg_pt.tilt_angle>-max_tilt):
00046       tilt_dir=-1
00047     else:
00048       tilt_dir=0
00049     if (data.buttons[joy_scroll_btn]==1):
00050        if (scroll_btn==0):
00051           current_robot=current_robot+1
00052           scroll_btn=1
00053           if (current_robot>num_robots):
00054              current_robot=1
00055           rospy.loginfo("Controlling robot with ID %d",current_robot)
00056     if (data.buttons[joy_scroll_btn]==0):
00057        scroll_btn=0
00058     if (data.buttons[joy_slow_btn]==1):
00059        if (slow_btn==0):
00060           if (slow==0.5):
00061              slow=1.0
00062           else:
00063              slow=0.5
00064           slow_btn=1
00065           rospy.loginfo("Max velocity is %f m/s",max_vel*slow)
00066     if (data.buttons[joy_slow_btn]==0):
00067        slow_btn=0
00068     if (data.buttons[joy_center_btn]==1):
00069        if (center_btn==0):
00070           msg_pt.pan_angle=0
00071           msg_pt.tilt_angle=0
00072           center=0
00073           center_btn=1
00074           rospy.loginfo("Centering pan tilt system")
00075     if (data.buttons[joy_center_btn]==0):
00076        center_btn=0
00077 
00078 
00079 def joy_cam():
00080     global pub 
00081     global pub_pt 
00082     global msg
00083     global msg_pt
00084     global max_pan
00085     global max_tilt
00086     global pan_vel
00087     global tilt_vel
00088     global pan_dir
00089     global tilt_dir
00090     global max_vel
00091     global max_rot
00092     global num_robots
00093     global scroll_btn
00094     global current_robot
00095     global joy_for
00096     global joy_rot
00097     global joy_pan
00098     global joy_tilt
00099     global joy_scroll_btn
00100     global joy_slow_btn
00101     global slow
00102     global slow_btn
00103     global joy_center_btn
00104     global center_btn
00105     global center
00106     joy_for = rospy.get_param("joy_for")
00107     joy_rot = rospy.get_param("joy_rot")
00108     joy_pan = rospy.get_param("joy_pan")
00109     joy_tilt = rospy.get_param("joy_tilt")
00110     joy_scroll_btn = rospy.get_param("joy_scroll_btn")
00111     joy_slow_btn = rospy.get_param("joy_slow_btn")
00112     joy_center_btn = rospy.get_param("joy_center_btn")
00113     max_vel = rospy.get_param("max_vel")
00114     max_rot = rospy.get_param("max_rot")
00115     max_pan = rospy.get_param("max_pan")
00116     max_tilt = rospy.get_param("max_tilt")
00117     num_robots = rospy.get_param("robots_n")
00118     robot_type = rospy.get_param("robot_type")
00119     center=10
00120     slow=1
00121     slow_btn=0
00122     center_btn=0
00123     pan_dir=0
00124     tilt_dir=0
00125     pan_vel=0.02
00126     tilt_vel=0.02
00127     pub=range(num_robots)
00128     pub_pt=range(num_robots)
00129     scroll_btn=0
00130     current_robot=1
00131     msg = Twist()
00132     msg.linear.x=0
00133     msg.linear.y=0
00134     msg.linear.z=0
00135     msg.angular.x=0
00136     msg.angular.y=0
00137     msg.angular.z=0
00138     msg_pt = ric_pan_tilt()
00139     msg_pt.pan_angle=0
00140     msg_pt.tilt_angle=0
00141     rospy.init_node('joy_cam', anonymous=True)
00142     rospy.Subscriber('joy', Joy, joy_callback)
00143     rospy.loginfo("Controlling robot with ID %d",current_robot)
00144     for i in range(1, num_robots+1):
00145       robot_name=robot_type+"_"+str(i)+"/cmd_vel"
00146       #rospy.loginfo(robot_name) 
00147       pub[i-1] = rospy.Publisher(robot_name, Twist)
00148       pub[i-1].publish(msg)
00149       rospy.sleep(0.1)
00150       robot_name_pt=robot_type+"_"+str(i)+"/pan_tilt"
00151       pub_pt[i-1] = rospy.Publisher(robot_name_pt, ric_pan_tilt)
00152       pub_pt[i-1].publish(msg_pt)
00153       rospy.sleep(0.1)
00154     rospy.loginfo("Ready to control, Controlling robot with ID: %d",current_robot)
00155     rospy.loginfo("Max velocity is %f m/s",max_vel*slow)
00156     while not rospy.is_shutdown():
00157       pub[current_robot-1].publish(msg)
00158       msg_pt.pan_angle=msg_pt.pan_angle+pan_dir*pan_vel
00159       msg_pt.tilt_angle=msg_pt.tilt_angle+tilt_dir*tilt_vel
00160       if (msg_pt.pan_angle>max_pan):
00161         msg_pt.pan_angle=max_pan
00162       if (msg_pt.pan_angle<-max_pan):
00163         msg_pt.pan_angle=-max_pan
00164       if (msg_pt.tilt_angle>max_tilt):
00165         msg_pt.tilt_angle=max_tilt
00166       if (msg_pt.tilt_angle<-max_tilt):
00167         msg_pt.tilt_angle=-max_tilt
00168       if (pan_dir<>0 or tilt_dir<>0):
00169         pub_pt[current_robot-1].publish(msg_pt)
00170       #rospy.loginfo(current_robot)
00171       rospy.sleep(0.1)
00172 
00173 if __name__ == '__main__':
00174     rospy.loginfo("Starting joy_cam.py node")
00175     joy_cam()
00176 


ric_base_station
Author(s): RoboTiCan
autogenerated on Thu Aug 27 2015 14:41:02