Started implementing main controller
Also sort of implemented the navigation Also tidy up the CMakeLists a little
This commit is contained in:
Pavel Lutskov
3
.gitignore
vendored
3
.gitignore
vendored
@@ -1,2 +1,5 @@
|
||||
# Vim stuff
|
||||
.*.sw*
|
||||
|
||||
# Some files
|
||||
experiments.py
|
||||
|
||||
186
CMakeLists.txt
186
CMakeLists.txt
@@ -1,13 +1,6 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(teleoperation)
|
||||
|
||||
## Add support for C++11, supported in ROS Kinetic and newer
|
||||
add_definitions(-std=c++11)
|
||||
# set(aruco_LIB /usr/local/lib/libaruco.so)
|
||||
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
cv_bridge
|
||||
image_transport
|
||||
@@ -18,193 +11,14 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
aruco
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
# catkin_python_setup()
|
||||
|
||||
################################################
|
||||
## Declare ROS messages, services and actions ##
|
||||
################################################
|
||||
|
||||
## To declare and build messages, services or actions from within this
|
||||
## package, follow these steps:
|
||||
## * Let MSG_DEP_SET be the set of packages whose message types you use in
|
||||
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend tag for "message_generation"
|
||||
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
|
||||
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
|
||||
## but can be declared for certainty nonetheless:
|
||||
## * add a run_depend tag for "message_runtime"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "message_generation" and every package in MSG_DEP_SET to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * add "message_runtime" and every package in MSG_DEP_SET to
|
||||
## catkin_package(CATKIN_DEPENDS ...)
|
||||
## * uncomment the add_*_files sections below as needed
|
||||
## and list every .msg/.srv/.action file to be processed
|
||||
## * uncomment the generate_messages entry below
|
||||
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
|
||||
|
||||
## Generate messages in the 'msg' folder
|
||||
# add_message_files(
|
||||
# FILES
|
||||
# Message1.msg
|
||||
# Message2.msg
|
||||
# )
|
||||
|
||||
## Generate services in the 'srv' folder
|
||||
# add_service_files(
|
||||
# FILES
|
||||
# Service1.srv
|
||||
# Service2.srv
|
||||
# )
|
||||
|
||||
## Generate actions in the 'action' folder
|
||||
# add_action_files(
|
||||
# FILES
|
||||
# Action1.action
|
||||
# Action2.action
|
||||
# )
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
# generate_messages(
|
||||
# DEPENDENCIES
|
||||
# sensor_msgs# std_msgs
|
||||
# )
|
||||
|
||||
################################################
|
||||
## Declare ROS dynamic reconfigure parameters ##
|
||||
################################################
|
||||
|
||||
## To declare and build dynamic reconfigure parameters within this
|
||||
## package, follow these steps:
|
||||
## * In the file package.xml:
|
||||
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
|
||||
## * In this file (CMakeLists.txt):
|
||||
## * add "dynamic_reconfigure" to
|
||||
## find_package(catkin REQUIRED COMPONENTS ...)
|
||||
## * uncomment the "generate_dynamic_reconfigure_options" section below
|
||||
## and list every .cfg file to be processed
|
||||
|
||||
## Generate dynamic reconfigure parameters in the 'cfg' folder
|
||||
# generate_dynamic_reconfigure_options(
|
||||
# cfg/DynReconf1.cfg
|
||||
# cfg/DynReconf2.cfg
|
||||
# )
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
## Declare things to be passed to dependent projects
|
||||
## INCLUDE_DIRS: uncomment this if you package contains header files
|
||||
## LIBRARIES: libraries you create in this project that dependent projects also need
|
||||
## CATKIN_DEPENDS: catkin_packages dependent projects also need
|
||||
## DEPENDS: system dependencies of this project that dependent projects also need
|
||||
catkin_package(
|
||||
# INCLUDE_DIRS include
|
||||
# LIBRARIES project_aruco
|
||||
# CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
# include_directories(include)
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
# add_library(${PROJECT_NAME}
|
||||
# src/${PROJECT_NAME}/project_aruco.cpp
|
||||
# )
|
||||
|
||||
## Add cmake target dependencies of the library
|
||||
## as an example, code may need to be generated before libraries
|
||||
## either from message generation or dynamic reconfigure
|
||||
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
# add_executable(${PROJECT_NAME}_node src/project_aruco_node.cpp)
|
||||
|
||||
## Rename C++ executable without prefix
|
||||
## The above recommended prefix causes long target names, the following renames the
|
||||
## target back to the shorter version for ease of user use
|
||||
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
|
||||
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
|
||||
|
||||
## Add cmake target dependencies of the executable
|
||||
## same as for the library above
|
||||
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
# target_link_libraries(${PROJECT_NAME}_node
|
||||
# ${catkin_LIBRARIES}
|
||||
# )
|
||||
|
||||
add_executable(aruco_detector src/aruco_detector.cpp)
|
||||
add_executable(aruco_effector src/aruco_effector.cpp)
|
||||
target_link_libraries(aruco_detector ${catkin_LIBRARIES} ${aruco_LIB})
|
||||
target_link_libraries(aruco_effector ${catkin_LIBRARIES} ${aruco_LIB})
|
||||
# add_dependencies(aruco tutorial_4_generate_messages_cpp)
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
## Mark executable scripts (Python etc.) for installation
|
||||
## in contrast to setup.py, you can choose the destination
|
||||
# install(PROGRAMS
|
||||
# scripts/my_python_script
|
||||
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
|
||||
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
# )
|
||||
|
||||
## Mark cpp header files for installation
|
||||
# install(DIRECTORY include/${PROJECT_NAME}/
|
||||
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
# FILES_MATCHING PATTERN "*.h"
|
||||
# PATTERN ".svn" EXCLUDE
|
||||
# )
|
||||
|
||||
## Mark other files for installation (e.g. launch and bag files, etc.)
|
||||
# install(FILES
|
||||
# # myfile1
|
||||
# # myfile2
|
||||
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
# )
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_project_aruco.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
|
||||
34
script/controller.py
Normal file
34
script/controller.py
Normal file
@@ -0,0 +1,34 @@
|
||||
from time import sleep
|
||||
|
||||
|
||||
handlers = {}
|
||||
STATE = 'idle' # Also walk, imitate and fallen
|
||||
|
||||
|
||||
def fall_handler():
|
||||
pass
|
||||
|
||||
|
||||
def walk_handler():
|
||||
pass
|
||||
|
||||
|
||||
def imitation_handler():
|
||||
pass
|
||||
|
||||
|
||||
def handle_transition(new_state):
|
||||
global STATE
|
||||
if STATE == 'walk':
|
||||
if new_state in ('fallen', 'idle'):
|
||||
STATE = new_state
|
||||
elif STATE == 'fallen':
|
||||
if new_state == 'idle':
|
||||
STATE = new_state
|
||||
elif STATE == 'imitate':
|
||||
STATE = new_state
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
pass
|
||||
# initialize stuff
|
||||
55
script/walker.py
Executable file
55
script/walker.py
Executable file
@@ -0,0 +1,55 @@
|
||||
#! /usr/bin/env python
|
||||
import os
|
||||
from time import sleep
|
||||
|
||||
from naoqi import ALProxy
|
||||
|
||||
import rospy
|
||||
import tf
|
||||
|
||||
FW = -0.30
|
||||
BK = -0.55
|
||||
LT = -0.55
|
||||
RT = 0.0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('walker')
|
||||
ll = tf.TransformListener()
|
||||
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
||||
mp.wakeUp()
|
||||
mp.moveInit()
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
sleep(0.3)
|
||||
try:
|
||||
trans, rot = ll.lookupTransform('Aruco_0_frame',
|
||||
'CameraTop_optical_frame',
|
||||
rospy.Time(0))
|
||||
except Exception as e:
|
||||
mp.stopMove()
|
||||
# inform_controller('walker', 'stop')
|
||||
continue
|
||||
print trans
|
||||
# continue
|
||||
|
||||
if BK < trans[2] < FW and LT < trans[0] < RT:
|
||||
mp.move(0, 0, 0)
|
||||
# inform_controller('walker', 'stop')
|
||||
continue
|
||||
|
||||
permission = True
|
||||
# permission = inform_controller('walker', 'move')
|
||||
if not permission:
|
||||
mp.stopMove()
|
||||
continue
|
||||
if trans[2] < BK: # backwards
|
||||
mp.move(-1, 0, 0)
|
||||
elif FW < trans[2]: # forwards
|
||||
mp.move(1, 0, 0)
|
||||
elif RT < trans[0]: # right
|
||||
mp.move(0, -1, 0)
|
||||
elif trans[0] < LT: # left
|
||||
mp.move(0, 1, 0)
|
||||
|
||||
mp.rest()
|
||||
@@ -1,6 +1,7 @@
|
||||
#include <ros/ros.h>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
@@ -26,7 +27,7 @@ class ImageConverter {
|
||||
it_(nh_)
|
||||
{
|
||||
// Subscrive to input video feed
|
||||
image_sub_ = it_.subscribe("/nao_robot/camera/front/image_raw", 1,
|
||||
image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
|
||||
&ImageConverter::imageCb, this);
|
||||
|
||||
// Create output windows
|
||||
@@ -42,7 +43,6 @@ class ImageConverter {
|
||||
|
||||
void imageCb(const sensor_msgs::ImageConstPtr& msg) {
|
||||
cv_bridge::CvImageConstPtr cv_ptr;
|
||||
ROS_INFO("IN IMAGE CB");
|
||||
try
|
||||
{
|
||||
// Create OpenCV, share memory with original message
|
||||
@@ -57,7 +57,6 @@ class ImageConverter {
|
||||
|
||||
// Output Original Image (bcs y not)
|
||||
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
|
||||
ROS_INFO("OUTPUT STUFF");
|
||||
|
||||
// Do Aruco
|
||||
cv::Mat cam_mat = (cv::Mat_<double>(3, 3) <<
|
||||
@@ -83,7 +82,12 @@ class ImageConverter {
|
||||
float y = markers[i].Tvec.at<float>(1);
|
||||
float z = markers[i].Tvec.at<float>(2);
|
||||
std::string id = "Aruco_";
|
||||
id += std::to_string(markers[i].id);
|
||||
|
||||
// int to str basically
|
||||
std::ostringstream ss;
|
||||
ss << markers[i].id;
|
||||
id += ss.str();
|
||||
|
||||
id += "_frame";
|
||||
tf::Transform aruco_tf;
|
||||
aruco_tf.setIdentity();
|
||||
@@ -93,8 +97,6 @@ class ImageConverter {
|
||||
id.c_str()));
|
||||
}
|
||||
cv::imshow(ARUCO_WINDOW, aruco_demo);
|
||||
|
||||
|
||||
cv::waitKey(2);
|
||||
}
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user