Go to the documentation of this file.00001
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
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
00171 rospy.sleep(0.1)
00172
00173 if __name__ == '__main__':
00174 rospy.loginfo("Starting joy_cam.py node")
00175 joy_cam()
00176