diff --git a/dxl_armed_turtlebot/launch/smallcircle.py b/dxl_armed_turtlebot/launch/smallcircle.py new file mode 100644 index 00000000..28414f4d --- /dev/null +++ b/dxl_armed_turtlebot/launch/smallcircle.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +import rospy #rosにおけるpythonのクライアントライブラリ +from opencv_apps.msg import RotatedRectStamped #opencv_apps/RotatedRectStampedのメッセージタイプを配信に再利用できる +from image_view2.msg import ImageMarker2 #image_view2/ImageMarker2のメッセージタイプを配信に再利用できる +from geometry_msgs.msg import Point #geometry_msgs/Pointのメッセージタイプを配信に再利用できる + +def cb(msg): + print msg.rect #受け取ったメッセージの矩形情報を垂れ流す + marker = ImageMarker2() #小さい丸のマーカーの型を作る + marker.type = 0 #マーカーの種類を決定 + marker.position = Point(msg.rect.center.x, msg.rect.center.y, 0) #マーカーの位置を決定 + pub.publish(marker) #作成されたメッセージ(marker)を現在のトピック(/Image_marker)に送信する + +rospy.init_node('client') #rospyにノード名をclientとして通知する +rospy.Subscriber('/camshift/track_box', RotatedRectStamped, cb) #ノードがopencv_apps.RotatedRectStampedの方のcbトピックから購読することを宣言している +pub = rospy.Publisher('/image_marker', ImageMarker2) #ノードが/image_markerのトピックにImageMarker2の型でメッセージを送っている +rospy.spin() #プロセスが終了するまで、ずっとノードを保持する diff --git a/dxl_armed_turtlebot/task/task3.py b/dxl_armed_turtlebot/task/task3.py new file mode 100755 index 00000000..c827bdd4 --- /dev/null +++ b/dxl_armed_turtlebot/task/task3.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python + +import rospy +from sensor_msgs.msg import Joy +from kobuki_msgs.msg import Led + +pub = rospy.Publisher('/mobile_base/commands/led1', Led) +def callback(data): + print(rospy.get_caller_id()+"joy\n %s", data.buttons) + if data.buttons[5] == 1: + pub.publish(data.buttons[4]+1) + return + + +def joy(): + rospy.init_node('myJoy', anonymous=True) + rospy.Subscriber("/joy", Joy, callback) + + print("spin") + rospy.spin() + +if __name__ == "__main__": + print("main") + joy() diff --git a/dxl_armed_turtlebot/task/task5.l b/dxl_armed_turtlebot/task/task5.l new file mode 100644 index 00000000..70289e45 --- /dev/null +++ b/dxl_armed_turtlebot/task/task5.l @@ -0,0 +1,18 @@ +(load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") +(dxl-armed-turtlebot-init) + +(ros::load-ros-manifest "sensor_msgs") +(ros::roseus "joylistener" :anonymous t) + +(defun joy-cb (msg) +(let ((buttons)) + (progn + (print (list 'joy (send msg :buttons))) + (setq buttons (send msg:buttons)) + (cond + (= buttons aref(buttons 4)) + (send *ri* :go-velocity 0.1 0 0) + ) + ) +) + diff --git a/lisp/#listener.l# b/lisp/#listener.l# new file mode 100755 index 00000000..7240d19d --- /dev/null +++ b/lisp/#listener.l# @@ -0,0 +1,19 @@ +#!/usr/bin/env roseus + +(ros::load-ros-manifest "kobuki_msgs") +(ros::roseus "listener" :anonymous t) + +;; callback function +(defun imudata-cb (msg) + (print (list 'button (send msg :button))) + (print (list 'state (send msg :state))) +) +(ros::subscribe "/mobile_base/events/button" kobuki_msgs::ButtonEvent #'imudata-cb) + + + +(ros::rate 10) +(do-until-key + (ros::sleep) + (ros::spin-once)) +(exit) diff --git a/lisp/listener.l b/lisp/listener.l new file mode 100755 index 00000000..7240d19d --- /dev/null +++ b/lisp/listener.l @@ -0,0 +1,19 @@ +#!/usr/bin/env roseus + +(ros::load-ros-manifest "kobuki_msgs") +(ros::roseus "listener" :anonymous t) + +;; callback function +(defun imudata-cb (msg) + (print (list 'button (send msg :button))) + (print (list 'state (send msg :state))) +) +(ros::subscribe "/mobile_base/events/button" kobuki_msgs::ButtonEvent #'imudata-cb) + + + +(ros::rate 10) +(do-until-key + (ros::sleep) + (ros::spin-once)) +(exit) diff --git a/turtleboteus/euslisp/listener.l b/turtleboteus/euslisp/listener.l new file mode 100755 index 00000000..7240d19d --- /dev/null +++ b/turtleboteus/euslisp/listener.l @@ -0,0 +1,19 @@ +#!/usr/bin/env roseus + +(ros::load-ros-manifest "kobuki_msgs") +(ros::roseus "listener" :anonymous t) + +;; callback function +(defun imudata-cb (msg) + (print (list 'button (send msg :button))) + (print (list 'state (send msg :state))) +) +(ros::subscribe "/mobile_base/events/button" kobuki_msgs::ButtonEvent #'imudata-cb) + + + +(ros::rate 10) +(do-until-key + (ros::sleep) + (ros::spin-once)) +(exit) diff --git a/turtleboteus/euslisp/task3.l b/turtleboteus/euslisp/task3.l new file mode 100755 index 00000000..9265d4c5 --- /dev/null +++ b/turtleboteus/euslisp/task3.l @@ -0,0 +1,55 @@ +(turtlebot-init) + +(ros::load-ros-manifest "sensor_msgs") +(ros::roseus "listener" :anonymous t) + +;; callback function +(defun battery-cb (msg) + (progn + (print (list 'laptopcharge (send msg :percentage))) + (when (< 50 (send msg :percentage)) + (send *ri* :publish-sound :error) +))) + +(ros::subscribe "/laptop_charge" sensor_msgs::BatteryState #'battery-cb) + + + +(setq state 0) +(setq wait 0) +(ros::rate 100) +(progn + (while t + ;(print (send *ri* :laptop-charge)) + (ros::spin-once) + ;(print "here") + (cond + ((= state 0) + (if (equal (send *ri* :state :bumper-vector) #f(0.0 0.0 0.0)) + (send *ri* :go-velocity 0 0 0) + (setq state 1) + ) + ) + ((= state 1) + (if (> wait 100) + (progn + (setq state 0) + (setq wait 0) + ) + (progn + (send *ri* :go-velocity -0.1 0 30) + (setq wait (+ wait 1)) + ) + ) + ) + (t + (print "default") + ) + ) + ) + (send *ri* :go-velocity 0 0 0) +) + + + +