Get position

controllers/hr46_b3m/main.cpp

@@ -38,6 +38,8 @@
 #include <webots/Gyro.hpp>
 #include <webots/Keyboard.hpp>
 #include <webots/Camera.hpp>
+#include <webots/Supervisor.hpp>
+#include <webots/Node.hpp>
 #include <cmath>
 #include <set>
 #include <exception>
@@ -317,7 +319,8 @@ class webots_motor_control
 
 private:
 	std::vector<std::pair<int32_t, std::string>> motors_info;
-	webots::Robot *robot;
+	webots::Supervisor *robot;
+	//webots::Robot *robot;
 	std::vector<std::tuple<int32_t, webots::Motor *, std::string>> robot_motors;
 	webots::Gyro *robot_gyro;
 	webots::Accelerometer *robot_accelerometer;
@@ -331,12 +334,19 @@ class webots_motor_control
 	//ここの大きさはreceive側と同じにする必要がある
 	const int32_t message_len;
 	uint32_t highest_priority;
+	//webots::Supervisor *super_visor;
+	webots::Node *node;
+	webots::Field *field;
+	bool is_position;
 
 public:
 	webots_motor_control() : mTimeStep(0), current_key(0), // forced_wait(waitForCreateQueue()),
-							 msgq(boost::interprocess::open_or_create, "WEBOTS_PICTURE_COMMUNICATION", 5, 1000), message_len(700 * 480 * 4), highest_priority(0)
+							 msgq(boost::interprocess::open_or_create, "WEBOTS_PICTURE_COMMUNICATION", 5, 1000), message_len(700 * 480 * 4), highest_priority(0), is_position(true)
 	{
-		robot = new webots::Robot();
+		std::cout << "construct in ---------------" << std::endl;
+		//robot = new webots::Robot();
+		robot = new webots::Supervisor();
+		//super_visor = new webots::Supervisor();
 		motors_info.push_back({FOOT_ROLL_R, "right_ankle_roll_joint"});
 		motors_info.push_back({LEG_PITCH_R, "right_ankle_pitch_joint"});
 		// motors_info.push_back({KNEE_R1, "right_ankle_pitch_mimic_joint"});
@@ -375,6 +385,15 @@ class webots_motor_control
 		reverse_motors.emplace("left_waist_pitch_joint");
 		reverse_motors.emplace("right_shoulder_roll_joint");
 		reverse_motors.emplace("left_shoulder_roll_joint");
+		std::cout << "getFromDef before ---------------" << std::endl;
+		//node = robot->getFromDef("head_yaw_joint");
+		init_supervisor();
+		if(node == NULL){
+			std::cerr << "get from def differ" << std::endl;
+			is_position = false;
+		}
+		//is_position = false;//add
+		std::cout << "getFromDef after ---------------" << std::endl;
 		for (auto &mp : motors_info)
 		{
 			auto motor_ptr = robot->getMotor(mp.second);
@@ -681,6 +700,39 @@ class webots_motor_control
 	{
 		return mTimeStep;
 	}
+
+	/*
+	const double *getPosition()
+	{
+		return node->getPosition();
+	}
+	*/
+
+	bool isPosition()
+	{
+		return is_position;
+	}
+
+	void init_supervisor()
+	{
+		node = robot->getRoot();
+		field = node->getField("children");
+		const int n = field->getCount();
+		for(int i=0;i<n;i++){
+			node = field->getMFNode(i);
+			std::cout << "-> " << node->getTypeName() << std::endl;
+		}
+
+		std::cout << "children n = " << n << std::endl;
+		std::cout << "name = " << field->getName() << std::endl;
+	}
+
+	const double *getPosition()
+	{
+		node = field->getMFNode(5);
+		return node->getPosition();
+	}
+
 };
 
 /*--------------------------------------*/
@@ -699,6 +751,8 @@ int main(int argc, char *argv[])
 
 	OrientationEstimator orientationEst((double)(FRAME_RATE) / 1000.0, 0.1);
 
+	const double *position;
+
 #endif
 	if (argc > 1)
 	{
@@ -720,6 +774,10 @@ int main(int argc, char *argv[])
 	// loop start
 	for (count_time_l = 0; wb_ganken.step(); count_time_l++)
 	{
+		if(wb_ganken.isPosition()){
+			position = wb_ganken.getPosition();
+			std::cout << "position = " << position[0] << "," << position[1] << "," << position[2] << std::endl;
+		}
 		bool cmd_accept = false;
 		{
 			// accept command

controllers/test/.gitignore

@@ -0,0 +1,2 @@
+build
+test

controllers/test/CMakeLists.txt

@@ -0,0 +1,65 @@
+project(test)
+cmake_minimum_required(VERSION 3.10)
+
+set(CMAKE_CXX_STANDARD 17)
+
+
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
+set(CMAKE_CXX_LINK_EXECUTABLE "${CMAKE_CXX_LINK_EXECUTABLE} -ldl")
+
+
+find_package(OpenCV REQUIRED)
+option(ENABLE_IMAGE "enable camera image" OFF)
+
+#add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../HCServer ./HCServer)
+#add_subdirectory(RTIMULib)
+
+
+#set(USE_VREP_SIMULATOR CACHE BOOL FORCE)
+#IF($ENV{VREP_DIR})
+#	set(USE_VREP_SIMULATOR True)
+#ENDIF($ENV{VREP_DIR})
+
+#IF(USE_VREP_SIMULATOR)
+#add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../SimDriver ./SimDriver)
+#add_definitions(-DVREP_SIMULATOR)
+#ENDIF(USE_VREP_SIMULATOR)
+
+
+if (DEFINED ENV{WEBOTS_HOME})
+    set(WEBOTS_HOME $ENV{WEBOTS_HOME})
+else()
+    set(WEBOTS_HOME /usr/local/webots)
+endif()
+
+set (WEBOTS_LIBRARIES Controller CppController)
+
+message(${WEBOTS_LIBRARIES})
+
+add_definitions(-D_AFXDLL -DSIM -D_CRT_SECURE_NO_WARNINGS)
+
+include_directories(
+	${CMAKE_BINARY_DIR}/gen
+	${CMAKE_CURRENT_SOURCE_DIR}
+	${WEBOTS_HOME}/include/controller/c 
+	${WEBOTS_HOME}/include/controller/cpp
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+link_directories(
+
+	${WEBOTS_HOME}/lib/controller
+${OpenCV_LIBRARIES}
+
+)
+
+
+
+add_executable(test
+    main.cpp
+)
+
+target_link_libraries(test ${OpenCV_LIBRARIES}
+	${WEBOTS_LIBRARIES}
+)

controllers/test/main.cpp

@@ -0,0 +1,108 @@
+#include <webots/Robot.hpp>
+#include <webots/Camera.hpp>
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include <chrono>
+#include <string>
+//#include <experimental/filesystem>
+#include <filesystem>
+
+//namespace fs = std::experimental::filesystem;
+namespace fs = std::filesystem;
+using namespace webots;
+
+// class ImageWriter
+// {
+// public:
+// 	ImageWriter();
+// 	~ImageWriter();
+// 	void createCapturedImageDirectory();
+// 	void saveCapturedImage(cv::Mat);
+// private:
+// 	bool enable_capture;
+// 	int image_count;
+// 	std::chrono::time_point<std::chrono::high_resolution_clock> last_capture_time;
+// 	std::string capture_save_path;
+// };
+//
+// ImageWriter::ImageWriter() : enable_capture(false), image_count(0)
+// {
+// 	createCapturedImageDirectory();
+// 	enable_capture = true;
+// 	last_capture_time = std::chrono::high_resolution_clock::now();
+// }
+//
+//
+// ImageWriter::~ImageWriter()
+// {
+// }
+//
+// void ImageWriter::createCapturedImageDirectory(void)
+// {
+//   std::string capture_save_path;
+//   std::string capture_dir("game_images");
+//   if(!fs::exists(capture_dir)) {
+//     fs::create_directory(capture_dir);
+//   }
+//   std::stringstream ss_capture;
+//   auto now_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
+//   ss_capture << capture_dir << "/" << std::put_time(std::localtime(&now_t), "%Y%m%dT%H%M%S");
+//   capture_save_path = ss_capture.str();
+//   fs::create_directory(capture_save_path);
+// }
+//
+// void ImageWriter::saveCapturedImage(cv::Mat img)
+// {
+//   static int image_count = 0;
+//   bool enable_capture = true;
+//   if(enable_capture) {
+//     const auto capture_interval = std::chrono::seconds(1);
+//     if(std::chrono::high_resolution_clock::now() - last_capture_time > capture_interval) {
+//       std::stringstream ss;
+//       ss << capture_save_path << "/" << std::setw(6) << std::setfill('0') << image_count << ".jpg";
+//       image_count++;
+//       cv::Mat save_img;
+//       cv::cvtColor(img, save_img, CV_YCrCb2BGR);
+//       cv::imwrite(ss.str().c_str(), save_img);
+//       last_capture_time = std::chrono::high_resolution_clock::now();
+//     }
+//   }
+// }
+
+int main() {
+  // ImageWriter imagewriter;
+  cv::Mat img(480, 640, CV_8UC3);
+  Robot *robot = new Robot();
+  Camera *camera;
+  camera = robot->getCamera("camera_sensor");
+  //camera->enable()
+  //camera->getImage();
+  camera->enable(13);
+  // imagewriter.createCapturedImageDirectory();
+
+  while(robot->step(32) != -1){
+    //mTimeStep = (int)robot->getBasicTimeStep();
+    //camera->enable(mTimeStep * 13);
+    const unsigned char *image = camera->getImage();
+    for(int x =0; x < 640; x++){
+      for(int y = 0;y <480;y++){
+        int r = camera->imageGetRed(image, 640, x, y);
+        int g = camera->imageGetGreen(image, 640, x, y);
+        int b = camera->imageGetBlue(image, 640, x, y);
+        img.at<cv::Vec3b>(y, x)[0] = b;
+        img.at<cv::Vec3b>(y, x)[1] = g;
+        img.at<cv::Vec3b>(y, x)[2] = r;
+      }
+    }
+
+    //std::cout << "hello world" << std::endl;
+    cv::imshow("test", img);
+    cv::waitKey(1);
+    // imagewriter.saveCapturedImage(img);
+  }
+
+    delete robot;
+    delete camera;
+
+    return 0;
+}

protos/GankenKun.proto

@@ -11,7 +11,7 @@ PROTO GankenKun [
   field  SFString    controller      "void"       # Is `Robot.controller`.
   field  MFString    controllerArgs  []           # Is `Robot.controllerArgs`.
   field  SFString    customData      ""           # Is `Robot.customData`.
-  field  SFBool      supervisor      FALSE        # Is `Robot.supervisor`.
+  field  SFBool      supervisor      TRUE        # Is `Robot.supervisor`.
   field  SFBool      synchronization TRUE         # Is `Robot.synchronization`.
   field  SFBool      selfCollision   TRUE         # Is `Robot.selfCollision`.
   field  SFInt32     cameraWidth     640          # Is `Camera.width`.

protos/test_camera.proto

@@ -0,0 +1,94 @@
+PROTO test_camera [
+field SFVec3f    translation 4 0.35 0.063504
+field SFRotation rotation       1 0 0 0
+field SFInt32    cameraWidth     640          # Is `Camera.width`.
+field SFInt32    cameraHeight    480          # Is `Camera.height`.
+field SFString   controller     "test"
+]
+{
+Robot {
+  translation IS translation
+  rotation IS rotation
+#  children [
+#    Shape {
+#        appearance PBRAppearance {
+#          baseColor 0.917647 0.145098 0.145098
+#          #roughness 1
+#          #metalness 0
+#        }
+#        geometry Box {
+#          size 0.1 0.05 0.585
+#        }
+#      #appearance PBRAppearance {
+#      #  baseColor 1 0 1
+#      #}
+#      #geometry Box {
+#      #  size 0.1 0.1 0.1
+#      #}
+#			
+#    }
+		children [
+			Transform {
+				translation 0.015000 -0.000000 0.085000
+				rotation -0.969980 0.000000 0.243186 3.141593
+				children [
+					Shape {
+						appearance DEF grey PBRAppearance {
+							baseColor 0.500000 0.500000 0.500000
+							transparency 0.000000
+							roughness 1.000000
+							metalness 0
+							emissiveColor 0.000000 0.000000 0.000000
+						}
+						geometry DEF head Box {
+						 size 0.068779 0.072919 0.050000
+						}
+					}
+				]
+			}
+		DEF camera_link Solid {
+			translation 0.036495 0.000000 0.063504 #変更
+			rotation 0.357407 -0.357407 -0.862856 1.717772
+			children [
+				Camera {
+					rotation 1.0 0.0 0.0 0.0
+					name "camera_sensor"
+					fieldOfView 2.1642
+					width IS cameraWidth
+					height IS cameraHeight
+					noise 0.0
+					spherical TRUE
+					focus Focus {
+						focalDistance 2.30
+						focalLength 16.8
+						maxFocalDistance 2.30
+						minFocalDistance 2.30
+					}
+				}
+			]
+      name "camera_link"
+		}
+		]
+		boundingObject Transform {
+			translation 0.015000 -0.000000 0.085000
+			rotation -0.969980 0.000000 0.243186 3.141593
+			children [
+				Box {
+					 size 0.068779 0.072919 0.050000
+				}
+			]
+		}
+#		physics Physics {
+#			density -1
+#			mass 0.111692
+#			centerOfMass [ 0.017325 0.000000 0.065059 ]
+#			inertiaMatrix [
+#				1.148365390e-04 8.968346100e-05 7.895650100e-05
+#				9.572310000e-07 -7.715580000e-07 2.138747600e-05
+#			]
+#		}
+
+	#boundingObject USE head
+	controller IS controller
+}
+}