diff --git a/detect_circle/.vscode/settings.json b/detect_circle/.vscode/settings.json new file mode 100755 index 0000000000..a3b2b51f1d --- /dev/null +++ b/detect_circle/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "vector": "cpp" + } +} \ No newline at end of file diff --git a/detect_circle/CMakeLists.txt b/detect_circle/CMakeLists.txt new file mode 100755 index 0000000000..e88da5f708 --- /dev/null +++ b/detect_circle/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.0.2) +project(detect_circle) + +find_package(catkin REQUIRED COMPONENTS + cv_bridge + roscpp + sensor_msgs + std_msgs + geometry_msgs + # message_generation +) + +# add_message_files( +# FILES +# PointStampedArray.msg +# ) + +# generate_messages( +# DEPENDENCIES +# std_msgs +# geometry_msgs +# ) + +# catkin_package( +# CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs message_runtime +# ) + +catkin_package( + CATKIN_DEPENDS cv_bridge roscpp sensor_msgs std_msgs geometry_msgs +) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +add_executable(${PROJECT_NAME}_node src/detect_circle.cpp) + +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) \ No newline at end of file diff --git a/detect_circle/launch/detect_circle.launch b/detect_circle/launch/detect_circle.launch new file mode 100755 index 0000000000..44f9cb4d8f --- /dev/null +++ b/detect_circle/launch/detect_circle.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + queue_size: 100 + + + + + + + + + + + + + + + + + + + + + diff --git a/detect_circle/msg/PointStampedArray.msg b/detect_circle/msg/PointStampedArray.msg new file mode 100755 index 0000000000..101ed8bc31 --- /dev/null +++ b/detect_circle/msg/PointStampedArray.msg @@ -0,0 +1,2 @@ +Header header +geometry_msgs/Point[] points \ No newline at end of file diff --git a/detect_circle/package.xml b/detect_circle/package.xml new file mode 100755 index 0000000000..eb141b51fc --- /dev/null +++ b/detect_circle/package.xml @@ -0,0 +1,74 @@ + + + detect_circle + 0.0.0 + The detect_circle package + + + + + mech-user + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + message_generation + + + + + + message_runtime + + + + + catkin + cv_bridge + roscpp + sensor_msgs + std_msgs + geometry_msgs + cv_bridge + roscpp + sensor_msgs + std_msgs + geometry_msgs + cv_bridge + roscpp + sensor_msgs + std_msgs + geometry_msgs + + + + + + + + diff --git a/detect_circle/src/detect_circle.cpp b/detect_circle/src/detect_circle.cpp new file mode 100755 index 0000000000..446412f3ca --- /dev/null +++ b/detect_circle/src/detect_circle.cpp @@ -0,0 +1,162 @@ +// #include "opencv2/imgcodecs.hpp" +// #include "opencv2/highgui.hpp" +// #include "opencv2/imgproc.hpp" +#include +#include +// #include +// #include +// #include +#include +// #include +#include + + + +// // initial and max values of the parameters of interests. +// const int canny_threshold_initial_value = 100; +// const int accumulator_threshold_initial_value = 50; +// // const int max_accumulator_threshold = 200; +// // const int max_canny_threshold = 255; + +// int canny_threshold = canny_threshold_initial_value; +// int accumulator_threshold = accumulator_threshold_initial_value; + +// class DetectCircle +// { +// private: +// ros::NodeHandle nh; +// // ros::NodeHandle pnh("~"); +// ros::Subscriber sub; +// ros::Publisher pub_img; +// ros::Publisher pub_points; + +// public: +// DetectCircle() +// { +// sub = nh.subscribe("/usb_cam/image_raw", 1, &DetectCircle::callback, this); +// pub_img = nh.advertise("/camera/image/circle", 1); +// pub_points = nh.advertise("/camera/points/circle", 1); +// pnh.getParam("canny_threshold", canny_threshold); +// pnh.getParam("accumulator_threshold", accumulator_threshold); +// } + +// ~DetectCircle(){}; + +// void callback(const sensor_msgs::ImageConstPtr& msg) +// { +// cv_bridge::CvImagePtr cv_ptr; +// try +// { +// cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); +// } +// catch(cv_bridge::Exception& e) +// { +// ROS_ERROR("cv_bridge exception: %s", e.what()); +// return; +// } + +// cv::Mat src, src_gray; + +// src = cv_ptr->image; + +// if(src.empty()) +// { +// std::cerr << "Invalid input image\n"; +// return; +// } + +// // Convert it to gray +// cv::cvtColor(src, src_gray, cv::COLOR_BGR2GRAY); + +// // Reduce the noise so we avoid false circle detection +// cv::GaussianBlur(src_gray, src_gray, cv::Size(9, 9), 2, 2); + +// detect_circle::PointStampedArray points; +// points.header = msg->header; + +// // will hold the results of the detection +// std::vector circles; +// // runs the actual detection +// cv::HoughCircles(src_gray, circles, cv::HOUGH_GRADIENT, 1, src_gray.rows/8, canny_threshold, accumulator_threshold, 0, 0); + +// // clone the colour, input image for displaying purposes +// cv::Mat img = src.clone(); +// points.points.resize(circles.size()); +// for(size_t i = 0; i < circles.size(); i++) +// { +// cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1])); +// int radius = cvRound(circles[i][2]); +// points.points[i].x = center.x; +// points.points[i].y = center.y; +// points.points[i].z = 0; +// // circle center +// cv::circle(img, center, 3, cv::Scalar(0,255,0), -1, 8, 0); +// // circle outline +// cv::circle(img, center, radius, cv::Scalar(0,0,255), 3, 8, 0); +// } + +// sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg(); + +// ROS_INFO("Number of circles detected: %ld", circles.size()); + +// for (size_t i = 0; i < circles.size(); i++) +// { +// ROS_INFO("Circle #%ld: center at (%d, %d)", i, (int)(points.points[i].x), (int)(points.points[i].y)); +// } + +// pub_img.publish(img_msg); +// pub_points.publish(points); +// } + +// }; + +class ConvertHoughCircle2PointStamped +{ + private: + ros::NodeHandle nh; + ros::Subscriber sub; + ros::Publisher pub; + + public: + ConvertHoughCircle2PointStamped() + { + sub = nh.subscribe("/hough_circles/circles", 1, &ConvertHoughCircle2PointStamped::callback, this); + pub = nh.advertise("/camera/circle/point", 1); + } + + ~ConvertHoughCircle2PointStamped(){}; + + void callback(const opencv_apps::CircleArrayStamped& msg) + { + geometry_msgs::PointStamped point; + if (msg.circles.size() == 0) + { + ROS_INFO("No circle detected"); + return; + } + point.header = msg.header; + point.point.x = msg.circles[0].center.x; + point.point.y = msg.circles[0].center.y; + point.point.z = 0; + + ROS_INFO("Circle center at (%f, %f)", point.point.x, point.point.y); + + pub.publish(point); + } + +}; + +int main(int argc, char** argv) +{ + // ros::init(argc, argv, "detect_circle"); + // DetectCircle dc; + // ROS_INFO("detect_circle node has started"); + // ros::spin(); + + ros::init(argc, argv, "detect_circle"); + ConvertHoughCircle2PointStamped chc2ps; + ROS_INFO("convert_hough_circle2point_stamped node has started"); + ros::spin(); + + return 0; +} diff --git a/jsk_2024_10_semi/CMakeLists.txt b/jsk_2024_10_semi/CMakeLists.txt new file mode 100644 index 0000000000..3c033f7f00 --- /dev/null +++ b/jsk_2024_10_semi/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_2024_10_semi) + +find_package(catkin REQUIRED COMPONENTS catkin_virtualenv) + +catkin_package() + +# Generate the virtualenv +catkin_generate_virtualenv( + PYTHON_INTERPRETER python3.10 + USE_SYSTEM_PACKAGES FALSE # Default TRUE + + # Provide extra arguments to the underlying pip invocation + #EXTRA_PIP_ARGS -vvv + ) + +install(DIRECTORY euslisp + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(FILES requirements.txt + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +catkin_install_python( + PROGRAMS scripts/conversation.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/jsk_2024_10_semi/README.md b/jsk_2024_10_semi/README.md new file mode 100644 index 0000000000..acd4388fa2 --- /dev/null +++ b/jsk_2024_10_semi/README.md @@ -0,0 +1,3 @@ +# jsk_2024_10_semi + + diff --git a/jsk_2024_10_semi/package.xml b/jsk_2024_10_semi/package.xml new file mode 100644 index 0000000000..4fa478aaa5 --- /dev/null +++ b/jsk_2024_10_semi/package.xml @@ -0,0 +1,22 @@ + + + jsk_2024_10_semi + 0.0.5 + The jsk_2024_10_semi package + + Kei Okada + + BSD + + catkin + + catkin_virtualenv + + fetcheus + jsk_maps + pr2eus + + + requirements.txt + + diff --git a/jsk_2024_10_semi/requirements.txt b/jsk_2024_10_semi/requirements.txt new file mode 100644 index 0000000000..1fecb40eaf --- /dev/null +++ b/jsk_2024_10_semi/requirements.txt @@ -0,0 +1,13 @@ +google-ai-generativelanguage==0.6.10 +google-api-core==2.22.0 +google-api-python-client==2.149.0 +google-auth==2.35.0 +google-auth-httplib2==0.2.0 +google-generativeai==0.8.3 +googleapis-common-protos==1.65.0 +protobuf==5.28.3 +grpcio==1.67.1 +grpcio-status==1.67.1 +# +speechrecognition +gtts diff --git a/jsk_2024_10_semi/scripts/catch.l b/jsk_2024_10_semi/scripts/catch.l new file mode 100644 index 0000000000..6b7a9a930b --- /dev/null +++ b/jsk_2024_10_semi/scripts/catch.l @@ -0,0 +1,314 @@ +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/pr2.l") +(load "package://pr2eus/pr2-interface.l") +(ros::roseus "pr2_send_joints") +(ros::load-ros-manifest "jsk_recognition_msgs") + +;;pr2生成 +(if (not (boundp '*pr2*)) (pr2-init)) +(setq *ri* (instance pr2-interface :init)) +;; 一辺600mmの立方体を出現させる +(setq *cube* (make-cube 600 600 600)) +;; 立方体を(700, 0, 300)移動 +(send *cube* :translate (float-vector 700 0 300)) +;;boxを適当な初期位置で配置 +(setq *target-box* (make-cube 20 20 20)) +(objects (list *pr2* *target-box* *cube*)) + +;; chabudaiフラグ +(setq *chabudai* nil) +;; ボールを回収した回数 +(setq *ball-count* 0) + +(setq *tfl* (instance ros::transform-listener :init)) +;;検知したboxとラベルを対応させるクラス +(defclass box-label-synchronizer + :super exact-time-message-filter) + +(defmethod box-label-synchronizer + (:callback (box-msg label-msg) + (print (list box-msg label-msg)) + (print (send-all (list box-msg label-msg) :header :stamp)) + (box-cb box-msg label-msg) + ) +) + +;;コールバック関数 +;;/docker/detic_segmentor/detected_classesというトピックにidの対応あり +(defun box-cb (box-msg label-msg) + (ros::ros-info "received ~A boxes, ~A labels" (length (send box-msg :boxes)) (length (send label-msg :labels))) + (dolist (msg-conbined (map cons #'(lambda (x y) (list x y)) (send box-msg :boxes) (send label-msg :labels))) + (let (box label) + (setq box (car msg-conbined) label (cadr msg-conbined)) + (ros::ros-info "box ~A, label ~A" (send box :pose) (send label :name)) + (when (or (string= (send label :name) "ball") (string= (send label :name) "soccer_ball") (string= (send label :name) "tennis_ball") (string= (send label :name) "beachball") (string= (send label :name) "ping-pong_ball")) + (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + (setq *target-dimensions* (send box :dimensions)) + (ros::ros-info "coords ~A, dimension ~A" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z))) + (send *target-box* :move-to *target-coords* :world) + (ros::ros-info "update target position") + ; ) + ) + ) + ) +) + +(defun chabudai-cb (msg) + (ros::ros-info "robot_mood: ~A" (send msg :data)) + (ros::ros-info "ball_count: ~A" *ball-count*) + (when (> (send msg :data) (- 9 *ball-count*)) + (setq *chabudai* t) + ) +) + +(defun init-pose () + ; (send *ri* :speak-jp "初期姿勢に移動します" :wait t) + (send *pr2* :reset-pose) + (send *pr2* :torso :waist-z :joint-angle 150) + (send *pr2* :larm :shoulder-p :joint-angle 0) + (send *pr2* :larm :shoulder-r :joint-angle 90) + (send *pr2* :head :neck-p :joint-angle 60) + (send *ri* :stop-grasp :arms) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) +) + +(defun chabudai-gaeshi () + (send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*) + ) + + (send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z + ) + + (send *pr2* :reset-pose) + (send *pr2* :torso :waist-z :joint-angle 150) + (send *pr2* :larm :collar-y :joint-angle 100) + (send *pr2* :rarm :collar-y :joint-angle -100) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *pr2* :larm :shoulder-p :joint-angle 74) + (send *pr2* :larm :elbow-p :joint-angle -50) + (send *pr2* :rarm :shoulder-p :joint-angle 74) + (send *pr2* :rarm :elbow-p :joint-angle -50) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ; (send *pr2* :larm :shoulder-p :joint-angle 70) + (send *pr2* :larm :shoulder-r :joint-angle 90) + (send *pr2* :larm :collar-y :joint-angle 60) + (send *pr2* :larm :elbow-p :joint-angle -110) + (send *pr2* :larm :elbow-r :joint-angle 180) + (send *pr2* :larm :wrist-p :joint-angle -25) + (send *pr2* :larm :wrist-r :joint-angle -15) + ; (send *pr2* :rarm :shoulder-p :joint-angle 70) + (send *pr2* :rarm :shoulder-r :joint-angle -90) + (send *pr2* :rarm :collar-y :joint-angle -60) + (send *pr2* :rarm :elbow-p :joint-angle -110) + (send *pr2* :rarm :elbow-r :joint-angle 180) + (send *pr2* :rarm :wrist-p :joint-angle -25) + (send *pr2* :rarm :wrist-r :joint-angle 15) + (send *pr2* :head :neck-p :joint-angle 50) + (send *ri* :stop-grasp :arms) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ; (send *pr2* + ; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + ; :translate (float-vector 0.0 0.5 0.0)) + ; :move-target (send *cube* :get :left-coords) + ; :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + ; :rotation-axis t) + ; (send *pr2* + ; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + ; :translate (float-vector 1.0 -0.5 0.0)) + ; :move-target (send *cube* :get :left-coords) + ; :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + ; :rotation-axis t) + + ; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + ; (send *ri* :wait-interpolation) + ; (send *irtviewer* :draw-objects) + + ; (send *pr2* + ; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + ; :translate (float-vector 0.0 -5.0 5.0)) + ; :move-target (send *cube* :get :left-coords) + ; :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + ; :rotation-axis t) + ; (send *pr2* + ; :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + ; :translate (float-vector 0.0 5.0 5.0)) + ; :move-target (send *cube* :get :left-coords) + ; :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + ; :rotation-axis t) + + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; sleep 1秒 + (unix:sleep 1) + + (send *pr2* :larm :shoulder-p :joint-angle -20) + (send *pr2* :larm :shoulder-r :joint-angle 20) + (send *pr2* :larm :elbow-p :joint-angle -100) + (send *pr2* :rarm :shoulder-p :joint-angle -20) + (send *pr2* :rarm :shoulder-r :joint-angle -20) + (send *pr2* :rarm :elbow-p :joint-angle -100) + (send *pr2* :head :neck-p :joint-angle -10) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) +) + +(defun pick-up () + (setq is-pickup-successful nil) + + (while (not is-pickup-successful) + (send *target-box* :newcoords (make-coords)) + ;;サブスクライブ + ; (ros::subscribe "/pointcloud_screenpoint/output_point" geometry_msgs::PointStamped #'callback) + (setq box-sync (instance box-label-synchronizer :init + (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) + (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) + + ;;ボールが見つかるまでループ + ; (send *ri* :speak-jp "ボールをさがしています" :wait t) + (ros::rate 3) ;; 3Hz + (while (< (elt (send *target-box* :worldpos) 2) 400) + (ros::ros-info "waiting... ~A" (send *target-box* :worldpos)) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once) + (ros::sleep) + ) + ; (send *ri* :speak-jp "ボールを見つけました" :wait t) + (ros::ros-info "start grasping... ~A" (send *target-box* :worldpos)) + (send *pr2* :larm :shoulder-p :joint-angle 0) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + + ;;ボールの上空へ移動 + (setq arm :larm) + + (send *pr2* arm :elbow-p :joint-angle -100) + (send *pr2* arm :collar-y :joint-angle 20) + + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1500) + (send *ri* :wait-interpolation) + + (send *pr2* arm :inverse-kinematics + (send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 150)) :rotate (deg2rad 90) :y) + :rotation-axis t + ) + + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1500) + (send *ri* :wait-interpolation) + + ;;ボールへ移動 + (send *pr2* arm :inverse-kinematics + (send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 -20)) :rotate (deg2rad 90) :y) + :rotation-axis t + ) + + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + + ;;掴む + (send *ri* :start-grasp arm :wait t) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + + (setq is-pickup-successful (> (send *ri* :robot arm :gripper :joint-angle) 5)) + (when (not is-pickup-successful) + (ros::ros-info "pickup failed ~%") + + ; (send *ri* :speak-jp "ボールを掴めませんでした" :wait t) + (send *pr2* arm :inverse-kinematics + (send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 100)) :rotate (deg2rad 90) :y) + :rotation-axis t + ) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (init-pose) + ) + ) + + (send *pr2* arm :inverse-kinematics + (send (send (send *target-box* :copy-worldcoords) :translate #f(30 0 100)) :rotate (deg2rad 90) :y) + :rotation-axis t + ) + + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + + (ros::ros-info "pickup successful ~%") + ; (send *ri* :speak-jp "ボールを持ち上げました" :wait t) + + ;; + (send *pr2* :larm :collar-y :joint-angle 50) + (send *pr2* :larm :shoulder-p :joint-angle 0) + (send *pr2* :larm :shoulder-r :joint-angle 90) + (send *pr2* :larm :elbow-p :joint-angle -30) + (send *pr2* :larm :elbow-r :joint-angle 90) + (send *pr2* :larm :wrist-p :joint-angle -10) + (send *pr2* :larm :wrist-r :joint-angle 0) + + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + ;;離す + (send *ri* :stop-grasp :arms) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) +) + + +; (init-pose) +; (unix:sleep 1) +; (chabudai-gaeshi) +;;メイン処理の実行 +(ros::advertise "ball_count" std_msgs::Int32 1) +(ros::subscribe "/robot_mood" std_msgs::Int32 #'chabudai-cb) + +(send *ri* :speak-jp "ボールを片付けます" :wait t) +(while (not *chabudai*) + (x::window-main-one) + (ros::ros-info "init-pose") + (init-pose) + (unix:sleep 1) + (ros::ros-info "pick up") + (pick-up) + + (setq *ball-count* (+ *ball-count* 1)) + (setq msg (instance std_msgs::Int32 :init)) + (send msg :data *ball-count*) + (ros::ros-info "ball_count: ~A" *ball-count*) + (ros::publish "ball_count" msg) + (ros::spin-once) + + (when *chabudai* + (chabudai-gaeshi) + ) +) \ No newline at end of file diff --git a/jsk_2024_10_semi/scripts/chabudai.l b/jsk_2024_10_semi/scripts/chabudai.l new file mode 100644 index 0000000000..01094a7593 --- /dev/null +++ b/jsk_2024_10_semi/scripts/chabudai.l @@ -0,0 +1,144 @@ +(require "package://pr2eus/pr2.l") ;;import pr2 package +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(ros::load-ros-manifest "roseus") +(if (not (boundp '*pr2*)) (pr2-init)) + +;; 一辺600mmの立方体を出現させる +(setq *cube* (make-cube 600 600 600)) +;; 立方体を(700, 0, 300)移動 +(send *cube* :translate (float-vector 700 0 300)) +;; chabudaiフラグ +(setq *chabudai* nil) + +;;Set coordination. +(send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*)) + +(send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + +(send *pr2* :reset-pose) ;; 初期姿勢 +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(objects (list *pr2* *cube*)) + + +(defun callback (msg) + (format t "robot_mood: ~A~%" (send msg :data)) + (when (< (send msg :data) 4) + (setq *chabudai* t) + ) +) + +(defun chabudai-gaeshi () + (send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*) + ) + + (send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z + ) + + (send *pr2* :reset-pose) + (send *pr2* :larm :collar-y :joint-angle 100) + (send *pr2* :rarm :collar-y :joint-angle -100) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *pr2* :larm :shoulder-p :joint-angle 70) + (send *pr2* :larm :elbow-p :joint-angle -50) + (send *pr2* :rarm :shoulder-p :joint-angle 70) + (send *pr2* :rarm :elbow-p :joint-angle -50) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ; (send *pr2* :larm :shoulder-p :joint-angle 70) + (send *pr2* :larm :shoulder-r :joint-angle 60) + (send *pr2* :larm :collar-y :joint-angle 32) + (send *pr2* :larm :elbow-p :joint-angle -110) + (send *pr2* :larm :elbow-r :joint-angle 180) + (send *pr2* :larm :wrist-p :joint-angle -30) + (send *pr2* :larm :wrist-r :joint-angle -30) + ; (send *pr2* :rarm :shoulder-p :joint-angle 70) + (send *pr2* :rarm :shoulder-r :joint-angle -60) + (send *pr2* :rarm :collar-y :joint-angle -32) + (send *pr2* :rarm :elbow-p :joint-angle -110) + (send *pr2* :rarm :elbow-r :joint-angle 180) + (send *pr2* :rarm :wrist-p :joint-angle -30) + (send *pr2* :rarm :wrist-r :joint-angle 30) + (send *pr2* :head :neck-p :joint-angle 50) + (send *ri* :stop-grasp :arms) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 1.0 0.5 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t) + (send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 1.0 -0.5 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis t) + + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 2.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t) + (send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 2.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis t) + + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; sleep 1秒 + (unix:sleep 1) + + (send *pr2* :larm :shoulder-p :joint-angle -20) + (send *pr2* :larm :shoulder-r :joint-angle 20) + (send *pr2* :larm :elbow-p :joint-angle -100) + (send *pr2* :rarm :shoulder-p :joint-angle -20) + (send *pr2* :rarm :shoulder-r :joint-angle -20) + (send *pr2* :rarm :elbow-p :joint-angle -100) + (send *pr2* :head :neck-p :joint-angle -10) + (send *ri* :angle-vector (send *pr2* :angle-vector) 500) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) +) + +(ros::subscribe "/robot_mood" std_msgs::Int32 #'callback) + +(do-until-key + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once) + (when *chabudai* + (chabudai_gaeshi) + (setq *chabudai* nil) + ) +) \ No newline at end of file diff --git a/jsk_2024_10_semi/scripts/chabudai_sim.l b/jsk_2024_10_semi/scripts/chabudai_sim.l new file mode 100644 index 0000000000..68977ea550 --- /dev/null +++ b/jsk_2024_10_semi/scripts/chabudai_sim.l @@ -0,0 +1,112 @@ +(require "package://pr2eus/pr2.l") ;;import pr2 package +; (require "package://pr2eus/pr2-utils.l") +; (require "package://pr2eus/pr2-interface.l") +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) + +;; 一辺600mmの立方体を出現させる +(setq *cube* (make-cube 100 100 100)) +;; 立方体を(700, 0, 300)移動 +(send *cube* :translate (float-vector 700 0 300)) + +;;Set coordination. +(send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*)) + +(send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + +; (send *pr2* :reset-pose) ;; 初期姿勢 +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(objects (list *pr2* *cube*)) + + +(send *pr2* :larm :shoulder-p :joint-angle 70) +(send *pr2* :larm :shoulder-r :joint-angle 60) +(send *pr2* :larm :collar-y :joint-angle 32) +(send *pr2* :larm :elbow-p :joint-angle -110) +(send *pr2* :larm :elbow-r :joint-angle 180) +(send *pr2* :larm :wrist-p :joint-angle -30) +(send *pr2* :larm :wrist-r :joint-angle -40) +(send *pr2* :rarm :shoulder-p :joint-angle 70) +(send *pr2* :rarm :shoulder-r :joint-angle -60) +(send *pr2* :rarm :collar-y :joint-angle -32) +(send *pr2* :rarm :elbow-p :joint-angle -110) +(send *pr2* :rarm :elbow-r :joint-angle 180) +(send *pr2* :rarm :wrist-p :joint-angle -30) +(send *pr2* :rarm :wrist-r :joint-angle 40) +(send *pr2* :head :neck-p :joint-angle 50) +; (send *ri* :stop-grasp :arms) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.5 0.0 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.5 0.0 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +; (send *ri* :start-grasp :arms) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; sleep 2秒 +(unix:sleep 2) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector -0.5 0.0 0.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector -0.5 0.0 0.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :rarm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + + +(send *pr2* :larm :shoulder-p :joint-angle 0) +(send *pr2* :larm :shoulder-r :joint-angle 5) +(send *pr2* :rarm :shoulder-p :joint-angle 0) +(send *pr2* :rarm :shoulder-r :joint-angle -5) +(send *pr2* :head :neck-p :joint-angle -10) +; (send *ri* :stop-grasp :arms) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 250) +; (send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) diff --git a/jsk_2024_10_semi/scripts/conversation.py b/jsk_2024_10_semi/scripts/conversation.py new file mode 100644 index 0000000000..a6aae5e4b9 --- /dev/null +++ b/jsk_2024_10_semi/scripts/conversation.py @@ -0,0 +1,304 @@ +# import os +# import sys +# import google.generativeai as genai +# from google.generativeai.types import HarmCategory, HarmBlockThreshold +# # import speech_recognition as sr +# # from gtts import gTTS +# import rospy +# from std_msgs.msg import String, Int32 +# from sound_play.msg import SoundRequest +# from speech_recognition_msgs.msg import SpeechRecognitionCandidates +# from difflib import SequenceMatcher + +# # ~/.bashrcに"export GEMINI_API_KEY='自分のapi_key'"を書いている +# GEMINI_API_KEY=os.getenv('GEMINI_API_KEY') +# MAX_TURNS = 20 + +# # def recognize_speech(): +# # # 音声認識のためのRecognizerオブジェクトを作成 +# # recognizer = sr.Recognizer() +# # # マイクから音声を取得 +# # with sr.Microphone() as source: +# # print('\033[36m'+"マイクの入力を待っています..."+'\033[0m') +# # recognizer.adjust_for_ambient_noise(source) # 周囲の雑音を調整 +# # audio = recognizer.listen(source) +# # try: +# # # 音声を日本語で認識 +# # text = recognizer.recognize_google(audio, language='ja-JP') +# # print('\033[36m'+f"認識結果: {text}"+'\033[0m') +# # return text +# # except sr.UnknownValueError: +# # print('\033[36m'+"音声を認識できませんでした。"+'\033[0m') +# # return None +# # except sr.RequestError as e: +# # print('\033[36m'+f"音声認識サービスに問題があります: {e}"+'\033[0m') +# # return None + +# # def text_to_speech_japanese(text): +# # # 日本語テキストを音声に変換 +# # tts = gTTS(text=text, lang='ja') +# # # 一時的な音声ファイルを保存 +# # filename = 'temp_audio.mp3' +# # tts.save(filename) +# # os.system(f"mpg123 {filename}") + + +# class GenerativeModel: +# def __init__(self, api_key, tools=[], model_name='gemini-1.5-pro'): +# genai.configure(api_key=api_key) +# if len(tools) > 0: +# self.model = genai.GenerativeModel(model_name=model_name, tools=tools) +# else: +# self.model = genai.GenerativeModel(model_name=model_name) +# self.safety_settings = { +# HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: HarmBlockThreshold.BLOCK_ONLY_HIGH, +# HarmCategory.HARM_CATEGORY_HATE_SPEECH: HarmBlockThreshold.BLOCK_ONLY_HIGH, +# HarmCategory.HARM_CATEGORY_HARASSMENT: HarmBlockThreshold.BLOCK_ONLY_HIGH, +# HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT:HarmBlockThreshold.BLOCK_ONLY_HIGH +# } +# """ +# 会話の安全性設定 +# HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: セクシャルな表現 +# HarmCategory.HARM_CATEGORY_HATE_SPEECH: 憎悪表現 +# HarmCategory.HARM_CATEGORY_HARASSMENT: 嫌がらせ +# HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT: 危険な表現 +# HarmBlockThreshold.BLOCK_NONE: ブロックしない +# HarmBlockThreshold.BLOCK_ONLY_HIGH: 高いレベルの有害な表現をブロック +# HarmBlockThreshold.BLOCK_MEDIUM_AND_ABOVE: 中以上のレベルの有害な表現をブロック +# HarmBlockThreshold.BLOCK_LOW_AND_ABOVE: 低以上のレベルの有害な表現をブロック +# HarmBlockThreshold.HARM_BLOCK_THRESHOLD_UNSPECIFIED: 未指定、デフォルトの設定 +# """ +# if len(tools) > 0: +# self.chat = self.model.start_chat(history=[], enable_automatic_function_calling=True) +# else: +# self.chat = self.model.start_chat(history=[]) +# def send_pre_prompt(self, pre_prompt): +# self.chat.send_message(pre_prompt, safety_settings=self.safety_settings) +# def send_message(self, message): +# response = self.chat.send_message(message, safety_settings=self.safety_settings) +# return response +# def get_history(self): +# return self.chat.history + +# # def conversation(): +# # input_mode = "text" +# # args = sys.argv +# # if len(args) > 2: +# # print("Usage: python conversation.py [input_mode]") +# # return +# # elif len(args) == 2: +# # if args[1] == "speech": +# # input_mode = "speech" +# # elif args[1] == "text": +# # input_mode = "text" +# # else: +# # print("Usage: input_mode should be either 'speech' or 'text'") +# # return +# # # モデルの初期化 +# # chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[set_mood_level], model_name='gemini-pro') +# # pre_prompt = "# 設定 \ +# # * あなた(gemini)はuserと対話するAIです。 \ +# # * userはあなたに話しかけることができます。 \ +# # * userが話しかけると、あなたはuserに返答します。 \ +# # # 制約\ +# # * あなたはuserのメッセージと、前回のmood_levelに基づいて、AIの機嫌を表すmood_levelを設定します。 \ +# # * mood_levelの初期値は5です。 \ +# # * mood_levelは1から9の整数で表されます。 \ +# # * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ +# # * 返答のテキストにはmood_levelについての情報を含めてはいけません。 \ +# # # 注意 \ +# # * あなたはuserのメッセージに対して、適切な返答を行う必要があります。" +# # chat_ai.send_pre_prompt(pre_prompt) +# # for turn in range(MAX_TURNS): +# # user_input = None +# # if input_mode == "text": +# # user_input = input("User >> ") +# # elif input_mode == "speech": +# # user_input = recognize_speech() +# # if user_input is None: +# # print("テキスト入力モードに切り替えます。") +# # input_mode = "text" +# # user_input = input("User >> ") +# # if user_input == "exit": +# # break +# # response = chat_ai.send_message(user_input) +# # print("AI >>", response.text) +# # if input_mode == "speech": +# # text_to_speech_japanese(response) +# # print(chat_ai.get_history()) + +# class Responder(): +# def __init__(self): +# # self.sub = rospy.Subscriber('test_input_text', String, self.callback) +# self.sub = rospy.Subscriber('/speech_to_text', SpeechRecognitionCandidates, self.callback, queue_size=1) +# self.pub = rospy.Publisher('/robotsound_jp', SoundRequest, queue_size=1) +# self.pub2 = rospy.Publisher('/robot_mood', Int32, queue_size=1) +# self.chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[self.set_mood_level], model_name='gemini-pro') +# pre_prompt = "# 設定 \ +# * あなた(gemini)はuserと対話するAIです。 \ +# * userはあなたに話しかけることができます。 \ +# * userが話しかけた後、あなたはuserに返答します。 \ +# * あなたはロボットで、今は机の上のボールを片付けながらuserと会話しています。 \ +# # 制約 \ +# * あなたはuserのメッセージに基づいて、AIの機嫌を表すmood_levelを設定します。 \ +# * mood_levelの初期値は5です。 \ +# * mood_levelは1から9の整数で表されます。 \ +# * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ +# * 返答のテキストにはmood_level（気分レベル）についての情報を含めてはいけません。" +# self.chat_ai.send_pre_prompt(pre_prompt) +# self.last_speak_time = rospy.Time.now() +# self.robot_mood_level = 5 + +# def callback(self, data): +# global robot_mood_level +# rospy.loginfo(data.transcript) +# if rospy.Time.now() - self.last_speak_time < rospy.Duration(10): +# rospy.loginfo("skip") +# return +# response = self.chat_ai.send_message(data.transcript) +# rospy.loginfo(response.text) +# sound_req = SoundRequest() +# sound_req.sound = -3 +# sound_req.command = 1 +# sound_req.volume = 70.0 +# sound_req.arg = str(response.text) +# self.pub.publish(sound_req) +# self.last_speak_time = rospy.Time.now() +# robot_mood_level = Int32() +# robot_mood_level.data = self.robot_mood_level +# self.pub2.publish(robot_mood_level) + +# def publish(self, data): +# global robot_mood_level +# self.pub.publish(data) +# self.pub2.publish(robot_mood_level) + +# def set_mood_level(self, mood_level:int) -> int: +# """setting AI's mood level based on the messages from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. +# Args: +# message: the message from the user. +# Returns: +# mood_level: the mood level of the AI from 1 to 9. 1 is the worst mood (angry) and 9 is the best mood (happy). +# """ +# print(f"AI's mood level: {mood_level}") +# self.robot_mood_level = int(mood_level) + + +# if __name__ == "__main__": +# rospy.init_node('responder') +# responder = Responder() +# rospy.spin() + + +#!/usr/bin/env python3 +import os +import sys +import google.generativeai as genai +from google.generativeai.types import HarmCategory, HarmBlockThreshold +# import speech_recognition as sr +# from gtts import gTTS +import rospy +from std_msgs.msg import String, Int32 +from sound_play.msg import SoundRequest +from speech_recognition_msgs.msg import SpeechRecognitionCandidates +# from difflib import SequenceMatcher + +# ~/.bashrcに"export GEMINI_API_KEY='自分のapi_key'"を書いている +GEMINI_API_KEY=os.getenv('GEMINI_API_KEY') +MAX_TURNS = 20 + +class GenerativeModel: + def __init__(self, api_key, tools=[], model_name='gemini-1.5-pro'): + genai.configure(api_key=api_key) + if len(tools) > 0: + self.model = genai.GenerativeModel(model_name=model_name, tools=tools) + else: + self.model = genai.GenerativeModel(model_name=model_name) + self.safety_settings = { + HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_HATE_SPEECH: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_HARASSMENT: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT:HarmBlockThreshold.BLOCK_ONLY_HIGH + } + """ + 会話の安全性設定 + HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: セクシャルな表現 + HarmCategory.HARM_CATEGORY_HATE_SPEECH: 憎悪表現 + HarmCategory.HARM_CATEGORY_HARASSMENT: 嫌がらせ + HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT: 危険な表現 + HarmBlockThreshold.BLOCK_NONE: ブロックしない + HarmBlockThreshold.BLOCK_ONLY_HIGH: 高いレベルの有害な表現をブロック + HarmBlockThreshold.BLOCK_MEDIUM_AND_ABOVE: 中以上のレベルの有害な表現をブロック + HarmBlockThreshold.BLOCK_LOW_AND_ABOVE: 低以上のレベルの有害な表現をブロック + HarmBlockThreshold.HARM_BLOCK_THRESHOLD_UNSPECIFIED: 未指定、デフォルトの設定 + """ + if len(tools) > 0: + self.chat = self.model.start_chat(history=[], enable_automatic_function_calling=True) + else: + self.chat = self.model.start_chat(history=[]) + def send_pre_prompt(self, pre_prompt): + self.chat.send_message(pre_prompt, safety_settings=self.safety_settings) + def send_message(self, message): + response = self.chat.send_message(message, safety_settings=self.safety_settings) + return response + def get_history(self): + return self.chat.history + + +class Responder(): + def __init__(self): + # self.sub = rospy.Subscriber('test_input_text', String, self.callback) + self.sub = rospy.Subscriber('ball_count', Int32, self.callback, queue_size=1) + self.pub = rospy.Publisher('/robotsound_jp', SoundRequest, queue_size=1) + self.pub2 = rospy.Publisher('/robot_mood', Int32, queue_size=1) + self.chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[self.set_mood_level], model_name='gemini-pro') + pre_prompt = "# 設定 \ + あなた(gemini)はロボットで、今は机の上のボールを片付けています。 \ + 片付けたボールの数が整数で入力されるので、ボールを片付けるほどイライラしてください。 \ + # 制約 \ + あなたは入力に基づいて、AIの機嫌を表すmood_level(イライラ度)を設定します。 \ + mood_levelの初期値は5です。 \ + mood_levelは1から9の整数で表されます。 \ + mood_levelは9が不機嫌で、1が幸せです。 \ + 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ + 返答のテキストにはmood_levelについての情報を含めてはいけません。" + self.chat_ai.send_pre_prompt(pre_prompt) + self.last_speak_time = rospy.Time.now() + self.robot_mood_level = 5 + + def callback(self, data): + global robot_mood_level + rospy.loginfo(data.data) + response = self.chat_ai.send_message(str(data.data)) + rospy.loginfo(response.text) + sound_req = SoundRequest() + sound_req.sound = -3 + sound_req.command = 1 + sound_req.volume = 70.0 + sound_req.arg = str(response.text) + self.pub.publish(sound_req) + self.last_speak_time = rospy.Time.now() + robot_mood_level = Int32() + robot_mood_level.data = self.robot_mood_level + self.pub2.publish(robot_mood_level) + + def publish(self, data): + global robot_mood_level + self.pub.publish(data) + self.pub2.publish(robot_mood_level) + + def set_mood_level(self, mood_level:int) -> int: + """setting AI's mood level based on inputs from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. + Args: + number: the number of balls put away + Returns: + mood_level: the mood level of the AI from 1 to 9. 9 is the worst mood (angry) and 1 is the best mood (happy). + """ + print(f"AI's mood level: {mood_level}") + self.robot_mood_level = int(mood_level) + + +if __name__ == "__main__": + rospy.init_node('responder') + responder = Responder() + rospy.spin() diff --git a/jsk_2024_10_semi/scripts/demo-box.l b/jsk_2024_10_semi/scripts/demo-box.l new file mode 100644 index 0000000000..a5fa730352 --- /dev/null +++ b/jsk_2024_10_semi/scripts/demo-box.l @@ -0,0 +1,41 @@ +(require "package://pr2eus/pr2-interface.l") +(pr2-init) + +(defclass box-label-synchronizer + :super exact-time-message-filter) + +(defmethod box-label-synchronizer + (:callback (box label) + (print (list box label)) + (print (send-all (list box label) :header :stamp)) + )) + +;; ;; test +;; (setq *box-label* (instance box-label-synchronizer :init +;; (list (list "/docker/detic_segmentor/output/boxes" jsk_recognition_msgs::BoundingBoxArray) +;; (list "/docker/detic_segmentor/detected_classes" jsk_recognition_msgs::LabelArray)))) + +(ros::load-ros-manifest "jsk_recognition_msgs") +(setq *target-box* (make-cube 100 100 100)) +(objects (list *pr2* *target-box*)) +(defun box-cb (msg) + (ros::ros-info "received ~A boxes" (length (send msg :boxes))) + (dolist (box (send msg :boxes)) + (when (= (send box :label) 41) ;; 41 -> ball + (setq *target-coords* (send (ros::tf-pose->coords (send box :pose)) :copy-worldcoords)) + (setq *target-dimensions* (send box :dimensions)) + (format t "coords ~A, dimension ~A~%" (send *target-coords* :worldcoords) (* (send *target-dimensions* :x) (send *target-dimensions* :y) (send *target-dimensions* :z))) + (when (and (< (elt (send *target-coords* :worldpos) 2) 1000) + (> (elt (send *target-coords* :worldpos) 2) 600)) + (send *target-box* :move-to *target-coords* :world) + (print "update target position") + )))) + +(ros::subscribe "/synchronized_detic_label_boxes" jsk_recognition_msgs::BoundingBoxArray #'box-cb) +;;(ros::subscribe "/kinect_head/depth_registered/boxes" jsk_recognition_msgs::BoundingBoxArray #'box-cb) +(do-until-key + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (ros::spin-once) + ) diff --git a/jsk_2024_10_semi/scripts/gomoku.l b/jsk_2024_10_semi/scripts/gomoku.l new file mode 100644 index 0000000000..c388b1fd3d --- /dev/null +++ b/jsk_2024_10_semi/scripts/gomoku.l @@ -0,0 +1,96 @@ +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) + +; (send *pr2* :reset-pose) ;; 初期姿勢 + +;; 一辺600mmの立方体を出現させる +(setq *cube* (make-cube 600 600 600)) +;; 立方体を(700, 0, 300)移動 +(send *cube* :translate (float-vector 700 0 300)) +(objects (list *pr2* *cube*)) + +;; PR2の左腕を指定の角度に動かす +(send *pr2* :larm :shoulder-p :joint-angle 15) +(send *pr2* :larm :shoulder-r :joint-angle 90) +(send *pr2* :larm :collar-y :joint-angle 45) +(send *pr2* :larm :elbow-p :joint-angle -100) +(send *pr2* :larm :elbow-r :joint-angle 130) +(send *pr2* :larm :wrist-p :joint-angle -10) +(send *pr2* :larm :wrist-r :joint-angle 0) +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +;; inverse-kinematicsを使ってPR2の左腕を現在位置から相対的に(0, 0, 100)移動 +; (let ((target-coords (make-coords :pos (float-vector 0 0 100)))) +; (send *pr2* :larm :inverse-kinematics target-coords)) + +;;Set coordination. +(send *cube* :put :left-coords + (make-cascoords + :coords (send (send *cube* :copy-worldcoords) :translate (float-vector 700 0 300)) + :parent *cube*)) + +(send *pr2* :larm :inverse-kinematics + (send (send *cube* :get :left-coords) :copy-worldcoords) + :rotation-axis :z) + +; (send *ri* :stop-grasp :arms) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 -2.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +; (send *ri* :start-grasp :arms) + +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +(send *pr2* :larm :elbow-r :joint-angle 120) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 2.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 5.0 -5.0 0.0)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +(send *pr2* :larm :wrist-r :joint-angle 0) + +(send *irtviewer* :draw-objects) + +;; sleep 1秒 +(unix:sleep 1) + +(send *pr2* + :inverse-kinematics (send (send (send *cube* :get :left-coords) :copy-worldcoords) + :translate (float-vector 0.0 0.0 -2.5)) + :move-target (send *cube* :get :left-coords) + :link-list (send *pr2* :link-list (send (send *pr2* :larm :end-coords) :parent)) + :rotation-axis t + :debug-view t) + +(send *irtviewer* :draw-objects) diff --git a/jsk_2024_10_semi/scripts/respond.py b/jsk_2024_10_semi/scripts/respond.py new file mode 100755 index 0000000000..2ee07c40eb --- /dev/null +++ b/jsk_2024_10_semi/scripts/respond.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +import os +import sys +import google.generativeai as genai +from google.generativeai.types import HarmCategory, HarmBlockThreshold +# import speech_recognition as sr +# from gtts import gTTS +import rospy +from std_msgs.msg import String, Int32 +from sound_play.msg import SoundRequest +from speech_recognition_msgs.msg import SpeechRecognitionCandidates +# from difflib import SequenceMatcher + +# ~/.bashrcに"export GEMINI_API_KEY='自分のapi_key'"を書いている +GEMINI_API_KEY=os.getenv('GEMINI_API_KEY') +MAX_TURNS = 20 + +class GenerativeModel: + def __init__(self, api_key, tools=[], model_name='gemini-1.5-pro'): + genai.configure(api_key=api_key) + if len(tools) > 0: + self.model = genai.GenerativeModel(model_name=model_name, tools=tools) + else: + self.model = genai.GenerativeModel(model_name=model_name) + self.safety_settings = { + HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_HATE_SPEECH: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_HARASSMENT: HarmBlockThreshold.BLOCK_ONLY_HIGH, + HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT:HarmBlockThreshold.BLOCK_ONLY_HIGH + } + """ + 会話の安全性設定 + HarmCategory.HARM_CATEGORY_SEXUALLY_EXPLICIT: セクシャルな表現 + HarmCategory.HARM_CATEGORY_HATE_SPEECH: 憎悪表現 + HarmCategory.HARM_CATEGORY_HARASSMENT: 嫌がらせ + HarmCategory.HARM_CATEGORY_DANGEROUS_CONTENT: 危険な表現 + HarmBlockThreshold.BLOCK_NONE: ブロックしない + HarmBlockThreshold.BLOCK_ONLY_HIGH: 高いレベルの有害な表現をブロック + HarmBlockThreshold.BLOCK_MEDIUM_AND_ABOVE: 中以上のレベルの有害な表現をブロック + HarmBlockThreshold.BLOCK_LOW_AND_ABOVE: 低以上のレベルの有害な表現をブロック + HarmBlockThreshold.HARM_BLOCK_THRESHOLD_UNSPECIFIED: 未指定、デフォルトの設定 + """ + if len(tools) > 0: + self.chat = self.model.start_chat(history=[], enable_automatic_function_calling=True) + else: + self.chat = self.model.start_chat(history=[]) + def send_pre_prompt(self, pre_prompt): + self.chat.send_message(pre_prompt, safety_settings=self.safety_settings) + def send_message(self, message): + response = self.chat.send_message(message, safety_settings=self.safety_settings) + return response + def get_history(self): + return self.chat.history + + +class Responder(): + def __init__(self): + # self.sub = rospy.Subscriber('test_input_text', String, self.callback) + self.sub = rospy.Subscriber('ball_count', Int32, self.callback, queue_size=1) + self.pub = rospy.Publisher('/robotsound_jp', SoundRequest, queue_size=1) + self.pub2 = rospy.Publisher('/robot_mood', Int32, queue_size=1) + self.chat_ai = GenerativeModel(GEMINI_API_KEY, tools=[self.set_mood_level], model_name='gemini-pro') + pre_prompt = "# 設定 \ + * あなた(gemini)はロボットで、今は机の上のボールを片付けています。 \ + * 片付けたボールの数が整数で入力されるので、片付けても片付けが進まないことに対してイライラしてください。 \ + # 制約 \ + * あなたは入力に基づいて、AIの機嫌を表すmood_levelを設定します。 \ + * mood_levelの初期値は5です。 \ + * mood_levelは1から9の整数で表されます。 \ + * 毎回の返答の際には、mood_levelを表示する関数（set_mood_level）の呼び出しを必ず行います。 \ + * 返答のテキストにはmood_level（気分レベル）についての情報を含めてはいけません。" + self.chat_ai.send_pre_prompt(pre_prompt) + self.last_speak_time = rospy.Time.now() + self.robot_mood_level = 5 + + def callback(self, data): + global robot_mood_level + rospy.loginfo(data.data) + response = self.chat_ai.send_message(str(data.data)) + rospy.loginfo(response.text) + sound_req = SoundRequest() + sound_req.sound = -3 + sound_req.command = 1 + sound_req.volume = 70.0 + sound_req.arg = str(response.text) + self.pub.publish(sound_req) + self.last_speak_time = rospy.Time.now() + robot_mood_level = Int32() + robot_mood_level.data = self.robot_mood_level + self.pub2.publish(robot_mood_level) + + def publish(self, data): + global robot_mood_level + self.pub.publish(data) + self.pub2.publish(robot_mood_level) + + def set_mood_level(self, mood_level:int) -> int: + """setting AI's mood level based on inputs from the user and the previous mood level of the AI. default is 5. AI must call this function every time it responds to the user. + Args: + message: the message from the user. + Returns: + mood_level: the mood level of the AI from 1 to 9. 1 is the worst mood (angry) and 9 is the best mood (happy). + """ + print(f"AI's mood level: {mood_level}") + self.robot_mood_level = int(mood_level) + + +if __name__ == "__main__": + rospy.init_node('responder') + responder = Responder() + rospy.spin() diff --git a/jsk_2024_10_semi/scripts/test.l b/jsk_2024_10_semi/scripts/test.l new file mode 100644 index 0000000000..9802d41bdf --- /dev/null +++ b/jsk_2024_10_semi/scripts/test.l @@ -0,0 +1,29 @@ +(require "package://pr2eus/pr2.l") ;;import pr2 package +; (require "package://pr2eus/pr2-utils.l") +; (require "package://pr2eus/pr2-interface.l") +; (if (not (boundp '*pr2*)) (pr2-init)) ;; (setq *pr2* (pr2))) ;; (pr2) is alternated by *pr2* +(if (not (boundp '*pr2*)) (setq *pr2* (pr2))) + +(send *pr2* :reset-pose) ;; 初期姿勢 +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +(objects (list *pr2*)) + +(send *pr2* :larm :shoulder-p :joint-angle 50) +(send *pr2* :larm :shoulder-r :joint-angle -20) +(send *pr2* :larm :wrist-p :joint-angle 20) +(send *pr2* :rarm :shoulder-p :joint-angle 50) +(send *pr2* :rarm :shoulder-r :joint-angle 20) +(send *pr2* :rarm :wrist-p :joint-angle -20) +(send *pr2* :head :neck-p :joint-angle 50) +(send *irtviewer* :draw-objects) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) + +(send *pr2* :larm :shoulder-p :joint-angle 5) +(send *pr2* :rarm :shoulder-p :joint-angle 5) +(send *pr2* :head :neck-p :joint-angle -10) +(send *irtviewer* :draw-objects) +; (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +; (send *ri* :wait-interpolation) +