Skip to content
GitLab
Commit c35e60bf authored by Zhuoyu Zhang's avatar Zhuoyu Zhang
Browse files

1

parent c9e2cc6a
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt 0 → 100644
View file @ c35e60bf
# toplevel CMakeLists.txt for a catkin workspace
# catkin/cmake/toplevel.cmake
cmake_minimum_required(VERSION 3.0.2)
project(Project)
set(CATKIN_TOPLEVEL TRUE)
# search for catkin within the workspace
set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
execute_process(COMMAND ${_cmd}
RESULT_VARIABLE _res
OUTPUT_VARIABLE _out
ERROR_VARIABLE _err
OUTPUT_STRIP_TRAILING_WHITESPACE
ERROR_STRIP_TRAILING_WHITESPACE
)
if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
# searching fot catkin resulted in an error
string(REPLACE ";" " " _cmd_str "${_cmd}")
message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
endif()
# include catkin from workspace or via find_package()
if(_res EQUAL 0)
set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
# include all.cmake without add_subdirectory to let it operate in same scope
include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
add_subdirectory("${_out}")
else()
# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
# or CMAKE_PREFIX_PATH from the environment
if(NOT DEFINED CMAKE_PREFIX_PATH)
if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
if(NOT WIN32)
string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
else()
set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
endif()
endif()
endif()
# list of catkin workspaces
set(catkin_search_path "")
foreach(path ${CMAKE_PREFIX_PATH})
if(EXISTS "${path}/.catkin")
list(FIND catkin_search_path ${path} _index)
if(_index EQUAL -1)
list(APPEND catkin_search_path ${path})
endif()
endif()
endforeach()
# search for catkin in all workspaces
set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
find_package(catkin QUIET
NO_POLICY_SCOPE
PATHS ${catkin_search_path}
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
if(NOT catkin_FOUND)
message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
endif()
endif()
catkin_workspace()
hotwheelspkg/CMakeLists.txt 0 → 100755
View file @ c35e60bf
cmake_minimum_required(VERSION 3.0.2)
project(hotwheelspkg)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## 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
roscpp
rospy
std_msgs
message_generation
)
## 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 exec_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 exec_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
# servo.msg
# )
## Generate services in the 'srv' folder
add_service_files(
FILES
)
## 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
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 exec_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 your 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 hotwheelspkg
# CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/hotwheelspkg.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/hotwheelspkg_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}
# )
#############
## 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
catkin_install_python(PROGRAMS
# scripts/luob.py
# scripts/controller.py
# scripts/hotwheels.py
scripts/manager.py
# scripts/cv.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_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
launch/hotwheels.launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_hotwheelspkg.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)
hotwheelspkg/launch/hotwheels.launch 0 → 100755
View file @ c35e60bf
<launch>
<node pkg="hotwheelspkg" type="manager.py" name="base" output="screen" />
<node pkg="hotwheelspkg" type="controller.py" name="controller" output="screen" />
</launch>
hotwheelspkg/msg/servo.msg 0 → 100755
View file @ c35e60bf
string cmd
\ No newline at end of file
hotwheelspkg/package.xml 0 → 100755
View file @ c35e60bf
<?xml version="1.0"?>
<package format="2">
<name>hotwheelspkg</name>
<version>0.0.0</version>
<description>The hotwheelspkg package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="yyf@todo.todo">yyf</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/hotwheelspkg</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
hotwheelspkg/scripts/Camera.py 0 → 100755
View file @ c35e60bf
#!/usr/bin/env python
import cv2
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import numpy as np
import math
import time
import pyrealsense2 as rs
class CameraManager:
def __init__(self):
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 60)
self.config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 60)
def register(self):
self.pipeline.start(self.config)
# 图像服务函数，返回角度sita
def color_srv(self):
"""
寻找色块
"""
frames = self.pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
image_color = np.asanyarray(color_frame.get_data())
color_dist = {
'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
'blue': {'Lower': np.array([97, 224, 70]), 'Upper': np.array([124, 255, 255])},
'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
}
gs_frame = cv2.GaussianBlur(image_color, (5, 5), 0) # 高斯模糊
hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV) # 转化成HSV图像
erode_hsv = cv2.erode(hsv, None, iterations=2) # 腐蚀 粗的变细
inRange_hsv = cv2.inRange(erode_hsv, color_dist["blue"]['Lower'],
color_dist["blue"]['Upper']) # 识别颜色
cnts = cv2.findContours(inRange_hsv.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] # 寻找轮廓
try:
c = max(cnts, key=cv2.contourArea) # 寻找最大色块
except:
return 0
rect = cv2.minAreaRect(c) # 找最小的包裹色块的矩形
box = cv2.boxPoints(rect) # box的点
sumx = 0.0
sumy = 0.0
for pi in box:
sumx += pi[0]
sumy += pi[1]
center = (int(sumx / 4), int(sumy / 4))
sita = math.atan(math.tan(34.5 / 180 * 3.1415926) * (center[0] - 320) / 320) / 3.1415926 * 180
# print("center, sita = ", center, sita)
# image = gs_frame
# cv2.drawContours(image, [np.int0(box)], -1, (0, 255, 255), 2)
# cv2.circle(image, center, 7, (0, 0, 0), -1)
# cv2.imshow("a", image)
# cv2.waitKey(1)
return int(sita)
def depth_srv(self):
frames = self.pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
depth_image = np.asanyarray(depth_frame.get_data())
image_arr = np.array(depth_image)
# print(image_arr.shape[0])
divide_p = [image_arr.shape[1] // 4, image_arr.shape[1] // 2, image_arr.shape[1] * 3 // 4]
center_part = image_arr[:int(image_arr.shape[0] * 3 // 4), int(image_arr.shape[1] * 3 // 8) : int(image_arr.shape[1] * 5 // 8)]
center_block_cnt = np.sum(center_part < 400)
left_part = image_arr[int(image_arr.shape[0] / 2):, 0:divide_p[1]]
right_part = image_arr[int(image_arr.shape[0] / 2):, divide_p[1]:image_arr.shape[1]]
# center_part_mean = center_part.mean()
# center_per = center_block_cnt / center_part.size
left_part_mean = left_part.mean()
right_part_mean = right_part.mean()
return int(center_block_cnt), int(left_part_mean), int(right_part_mean)
if __name__ == "__main__":
import sys
print("camera", sys.version)
rospy.init_node("CameraManager")
camera = CameraManager()
rate = rospy.Rate(1000)
rospy.spin()
rate.sleep()
hotwheelspkg/scripts/FileManager.py 0 → 100755
View file @ c35e60bf
#!/usr/bin/env python
from asyncio import futures
from math import sin, cos
import sys
sys.path.append("/home/yyf/ros03/src/hotwheelspkg/scripts")
class FileManager:
FILE_PATH = '/home/yyf/ros03/recording/'
FILE_SUB_PATH = ''
FILE_NAME = 'default'
FILE_EXTENSION = '.save'
lines = []
num = 0
def __init__(self, filename=FILE_NAME, subpath=FILE_SUB_PATH, extension=FILE_EXTENSION):
self.FILE_NAME = filename
self.FILE_SUB_PATH = subpath
self.FILE_EXTENSION = extension
print(f'> File: 文件系统初始化: {self.FILE_PATH + self.FILE_SUB_PATH + "/" + self.FILE_NAME + self.FILE_EXTENSION}')
self.LoadFile()
def Show(self):
print("> FILE: INFO:")
print(f" File Path: {self.FILE_PATH + '/' + self.FILE_SUB_PATH}")
print(f" File Name: {self.FILE_NAME + self.FILE_EXTENSION}")
print(f" File Content: (Line Num: {self.num})")
for line in self.lines:
print(" ", line)
def LoadFile(self):
file = self.FILE_PATH + self.FILE_SUB_PATH + '/' + self.FILE_NAME + self.FILE_EXTENSION
try:
with open(file, 'r') as f:
self.lines = f.readlines()
self.num = len(self.lines)
# print('> File: 打开文件成功')
except:
with open(file, 'w') as f:
print("> File: 创建新文件")
def WriteFile(self, data, index=0):
while index >= len(self.lines):
self.lines.append('\n')
self.lines[index] = data
file = self.FILE_PATH + self.FILE_SUB_PATH + '/' + self.FILE_NAME + self.FILE_EXTENSION
with open(file, 'w') as f:
f.writelines(self.lines)
# print('> File: 写入文件成功')
def ChangeFile(self, filename):
self.FILE_NAME = filename
self.LoadFile()
def WriteFileAll(self, lines):
file = self.FILE_PATH + self.FILE_SUB_PATH + '/' + self.FILE_NAME + self.FILE_EXTENSION
with open(file, 'w') as f:
f.writelines(lines)
# print('> File: 写入时间流文件成功')
FilePool = {}
def GetFileObject(filename='default', subpath='', extension='.save'):
full_name = subpath + "/" + filename + extension
if full_name not in FilePool:
FilePool[full_name] = FileManager(filename, subpath, extension)
else:
FilePool[full_name].LoadFile()
return FilePool[full_name]
\ No newline at end of file
hotwheelspkg/scripts/FileOperation.py 0 → 100755
View file @ c35e60bf
#!/usr/bin/env python
import math
import sys
sys.path.append("/home/yyf/ros03/src/hotwheelspkg/scripts")
from manager import ServoManager as SM
from FileManager import *
from calculate import calc_pos
def MergeFlow(base_flow_name: str, adding_flow_name: str, generate_flow_name: str, index_group: list):
'''
将base_flow_name和adding_flow_name合并到generate_flow_name中
'''
generation = FileManager(generate_flow_name, "TimeFlow", ".flow")
base = FileManager(base_flow_name, "TimeFlow", ".flow")
adding = FileManager(adding_flow_name, "TimeFlow", ".flow")
base_lines = base.lines
adding_lines = adding.lines
len1 = len(base_lines)
len2 = len(adding_lines)
if(len1 < len2): # 原始Flow更短，则延拓最后一帧
final_frame = base_lines[-1]
for i in range(len2 - len1):
base_lines.append(final_frame)
new_lines = []
for line_index in range(len2):
data_new = base_lines[line_index][:-1].split(',')
data_adding = adding_lines[line_index][:-1].split(',')
for adding_servo_index in index_group:
data_new[adding_servo_index] = data_adding[adding_servo_index]
new_line = ','.join(data_new) + '\n'
new_lines.append(new_line)
generation.WriteFileAll(new_lines)
print(f'> FILE: 生成的Flow文件为: /TimeFlow/{generate_flow_name}.flow')
# base覆盖target
def TransLeg(source_flow_name: str, base_flow_name: str, new_name: str, base_servo_group: list, target_servo_group:list):
'''
从source中抽取出一组servo的数据, 并将其覆盖到target中, 保存为新的flow文件
'''
base = FileManager(base_flow_name, "TimeFlow", ".flow")
source = FileManager(source_flow_name, "TimeFlow", ".flow")
new = FileManager(new_name, "TimeFlow", ".flow")
new_lines = source.lines.copy()
base_lines = base.lines.copy()
for line_index in range(len(new_lines)):
data_new = new_lines[line_index][:-1].split(',')
data_base = base_lines[line_index][:-1].split(',')
for i in range(3):
data_base[target_servo_group[i] - 1] = data_new[base_servo_group[i] - 1]
new_lines[line_index] = ','.join(data_base) + '\n'
new.WriteFileAll(new_lines)
print(f'> FILE: 生成的Flow文件为: /TimeFlow/{new_name}.flow')
def MergeSomeFlow(base_flow_name: str, adding_flow_name: str, generate_flow_name: str, index_group: list, base_start_id: int, copy_length: int):
'''
将base_flow_name和adding_flow_name中的一段合并到generate_flow_name中
'''
generation = FileManager(generate_flow_name, "TimeFlow", ".flow")
base = FileManager(base_flow_name, "TimeFlow", ".flow")
adding = FileManager(adding_flow_name, "TimeFlow", ".flow")
base_lines = base.lines
adding_lines = adding.lines
len1 = len(base_lines)
len2 = len(adding_lines)
if(len1 < len2): # 原始Flow更短，则延拓最后一帧
final_frame = base_lines[-1]
for i in range(len2 - len1):
base_lines.append(final_frame)
new_lines = []
for line_index in range(copy_length):
data_new = base_lines[base_start_id + line_index][:-1].split(',')
data_adding = adding_lines[line_index][:-1].split(',')
for adding_servo_index in index_group:
data_new[adding_servo_index] = data_adding[adding_servo_index]
new_line = ','.join(data_new) + '\n'
new_lines.append(new_line)
generation.WriteFileAll(new_lines)
print(f'> FILE: 生成的Flow文件为: /TimeFlow/{generate_flow_name}.flow')
def SetFlowStep(base_flow_name: str, new_flow_name: str, step_id: int, new_step: list, alpha = 0.7):
'''
为了实现重设局部, 将改变点附近的帧全部随之改变, 变化率以alpha倍率衰减, 保存为新的flow文件
'''
base = FileManager(base_flow_name, "TimeFlow", ".flow")
new = FileManager(new_flow_name, "TimeFlow", ".flow")
base_lines = base.lines
new_lines = []
base_data = base_lines[step_id].split(',')
delta = [new_step[i] - base_data[i] for i in range(len(new_step))]
for line_index in range(len(base_lines)):
data_new = base_lines[line_index][:-1].split(',')
for i in range(len(new_step)):
data_new[i] = str(int(base_data[i]) + int(delta[i] * (alpha**abs(line_index - step_id))))
new_line = ','.join(data_new) + '\n'
new_lines.append(new_line)
new.WriteFileAll(new_lines)
print(f'> FILE: 生成的Flow文件为: /TimeFlow/{new_flow_name}.flow')
def ExpandRoll(file_name, times=5):
base = FileManager("roll", "Step")
new = FileManager(file_name, "Step")
base_lines = base.lines
new_lines = []
zero_pos = base_lines[0]
new_lines.append(zero_pos)
for i in range(times):
for line in base_lines[1:]:
new_lines.append(line)
new_lines.append(zero_pos)
new.WriteFileAll(new_lines)
print(f'> FILE: 完成')
if __name__ == "__main__":
# MergeFlow(sys.argv[1], sys.argv[2], "turn0", SM.RIGHT_BACK)
# TransLeg(source_flow_name='turndone', base_flow_name='turndone', new_name='turndone',\
# base_servo_group=SM.LEFT_FRONT, target_servo_group=SM.RIGHT_BACK)
# TransLeg('turn0', SM.LEFT_BACK, SM.LEFT_FRONT, 'turn0')
# TransLeg('turn0', SM.LEFT_BACK, SM.RIGHT_BACK, 'turn0')
# TransLeg('turn0', SM.LEFT_LEG, SM.RIGHT_LEG, 'turn0')
ExpandRoll('roll3')
\ No newline at end of file
hotwheelspkg/scripts/JoyController.py 0 → 100755
View file @ c35e60bf
#!/usr/bin/env python
import time
import sys
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Joy
avoid_flag = False
online_flag = False
mode_flag = False # 足式形态->滚轮形态
walk_delay = 0.3
roll_delay = 1.0
def joy_callback(data):
global avoid_flag, online_flag, mode_flag
global walk_delay, roll_delay
# print(data)
# axes向左为正，向上为正
# L2,R2默认为1.0, 按下为-1.0
# axes6，7由两组按钮上下左右控制
# print("axes0(LX): ", data.axes[0])
# print("axes1(LY): ", data.axes[1])
# print("axes2(L2): ", data.axes[2])
# print("axes3(RX): ", data.axes[3])
# print("axes4(RY): ", data.axes[4])
# print("axes5(R2): ", data.axes[5])
# print("axes6(十字左右): ", data.axes[6])
# print("axes7(十字上下): ", data.axes[7])
LX, LY, L2, RX, RY, R2, left_right, up_down = data.axes
left = left_right > 0
right = left_right < 0
up = up_down > 0
down = up_down < 0
L2 = L2 < 0
R2 = R2 < 0
# print("buttons0(A): ", data.buttons[0])
# print("buttons1(B): ", data.buttons[1])
# print("buttons2(X): ", data.buttons[2])
# print("buttons3(Y): ", data.buttons[3])
# print("buttons4(L1): ", data.buttons[4])
# print("buttons5(R1): ", data.buttons[5])
# print("buttons6(select): ", data.buttons[6])
# print("buttons7(start): ", data.buttons[7])
# print("buttons8(mode): ", data.buttons[8])
# print("buttons9(L3): ", data.buttons[9])
# print("buttons10(R3): ", data.buttons[10])
A, B, X, Y, L1, R1, select, start, mode, L3, R3 = data.buttons
#flag
msg = String()
if(start == 1):
if not mode_flag:
msg.data = "manager.Init()"
print(msg.data)
pub.publish(msg)
else:
msg.data = "manager.ReadAction('wheel')"
print(msg.data)
pub.publish(msg)
elif(select == 1):
msg.data = "manager.GetVol()"
print(msg.data)
pub.publish(msg)
elif(L3 == 1):
msg.data = "manager.ServoScan()"
print(msg.data)
pub.publish(msg)
elif(mode == 1):
mode_state = "TransForm()" if not mode_flag else "TransFormBack()"
mode_flag = not mode_flag
msg.data = "manager." + mode_state
print(msg.data)
pub.publish(msg)
elif(up and not mode_flag):
msg.data = "manager.WalkOnce(delay=" + str(walk_delay) + ")"
print(msg.data)
pub.publish(msg)
elif(left and not mode_flag):
msg.data = "manager.TurnLeft()"
print(msg.data)
pub.publish(msg)
elif(right and not mode_flag):
msg.data = "manager.TurnRight()"
print(msg.data)
pub.publish(msg)
elif(down and not mode_flag):
msg.data = "manager.WalkBackOnce(delay=" + str(walk_delay) + ")"
print(msg.data)
pub.publish(msg)
elif(A and not mode_flag):
avoid_state = "AvoidOn()" if not avoid_flag else "AvoidOff()"
avoid_flag = not avoid_flag
msg.data = "manager." + avoid_state
print(msg.data)
pub.publish(msg)
elif(X and not mode_flag):
online_state = "OnlineOn()" if not online_flag else "OnlineOff()"
online_flag = not online_flag
msg.data = "manager." + online_state
print(msg.data)
pub.publish(msg)
elif(Y and mode_flag):
msg.data = "manager.Roll(delay=" + str(roll_delay) + ")"
print(msg.data)
pub.publish(msg)
elif(B):
msg.data = "manager.DampAll()"
print(msg.data)
pub.publish(msg)
elif(L1 and not mode_flag):
walk_delay = walk_delay + 0.1 if walk_delay <= 0.3 else 0.4
msg.data = "manager.set_walk_delay(" + str(walk_delay) + ")"
print(msg.data)
pub.publish(msg)
elif(L2 and not mode_flag):
walk_delay = walk_delay - 0.1 if walk_delay >= 0.2 else 0.1
msg.data = "manager.set_walk_delay(" + str(walk_delay) + ")"
pub.publish(msg)
print(msg.data)
elif(R1 and mode_flag):
roll_delay = roll_delay + 0.05 if roll_delay <= 1.95 else 2.0
print("new roll_delay: ", roll_delay)
elif(R2 and mode_flag):
roll_delay = roll_delay - 0.05 if roll_delay >= 0.05 else 0.0
print("new roll_delay: ", roll_delay)
elif(R3):
mode_flag = not mode_flag
print("new mode_flag: ", mode_flag)
if __name__ == "__main__":
#2.初始化 ROS 节点:命名(唯一)
rospy.init_node("JoyController")
#3.实例化 发布者 对象
pub = rospy.Publisher("Manager", String, queue_size=1)
#4.组织被发布的数据，.并编写逻辑发布数据
sub = rospy.Subscriber("joy", Joy, joy_callback)
#5.循环发布数据
rospy.spin()
\ No newline at end of file
hotwheelspkg/scripts/RecordConfig.txt 0 → 100644
View file @ c35e60bf
左前腿: LEFT_FRONT 1 (1|1|1)
右前腿: RIGHT_FRONT 1 (1|1|1)
左后腿: LEFT_BACK 1 (1|1|1)
右后腿: RIGHT_BACK 1 (1|1|1)
左边腿: LEFT_LEG 1 (1|1|1)
右边腿: RIGHT_LEG 1 (1|1|1)
两侧肩: SHOUDER 1 (1|1)
\ No newline at end of file
hotwheelspkg/scripts/UDPComms.py 0 → 100755
View file @ c35e60bf
"""
This is a simple library to enable communication between different processes (potentially on different machines) over a network using UDP. It's goals a simplicity and easy of understanding and reliability
mikadam@stanford.edu
"""
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import socket
import struct
from collections import namedtuple
import msgpack
from sys import version_info
USING_PYTHON_2 = (version_info[0] < 3)
if USING_PYTHON_2:
from time import time as monotonic
else:
from time import monotonic
timeout = socket.timeout
MAX_SIZE = 65507
DEFAULT_IP = "10.0.0.255"
class Publisher:
def __init__(self, port, ip = DEFAULT_IP):
""" Create a Publisher Object
Arguments:
port -- the port to publish the messages on
ip -- the ip to send the messages to
"""
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
self.broadcast_ip = ip
self.sock.settimeout(0.2)
self.sock.connect((self.broadcast_ip, port))
self.port = port
def send(self, obj):
""" Publish a message. The obj can be any nesting of standard python types """
msg = msgpack.dumps(obj, use_bin_type=False)
assert len(msg) < MAX_SIZE, "Encoded message too big!"
self.sock.send(msg)
def __del__(self):
self.sock.close()
class Subscriber:
def __init__(self, port, timeout=0.2):
""" Create a Subscriber Object
Arguments:
port -- the port to listen to messages on
timeout -- how long to wait before a message is considered out of date
"""
self.max_size = MAX_SIZE
self.port = port
self.timeout = timeout
self.last_data = None
self.last_time = float('-inf')
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
if hasattr(socket, "SO_REUSEPORT"):
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
self.sock.settimeout(timeout)
self.sock.bind(("", port))
def recv(self):
""" Receive a single message from the socket buffer. It blocks for up to timeout seconds.
If no message is received before timeout it raises a UDPComms.timeout exception"""
try:
self.last_data, address = self.sock.recvfrom(self.max_size)
except BlockingIOError:
raise socket.timeout("no messages in buffer and called with timeout = 0")
self.last_time = monotonic()
return msgpack.loads(self.last_data, raw=USING_PYTHON_2)
def get(self):
""" Returns the latest message it can without blocking. If the latest massage is
older then timeout seconds it raises a UDPComms.timeout exception"""
try:
self.sock.settimeout(0)
while True:
self.last_data, address = self.sock.recvfrom(self.max_size)
self.last_time = monotonic()
except socket.error:
pass
finally:
self.sock.settimeout(self.timeout)
current_time = monotonic()
if (current_time - self.last_time) < self.timeout:
return msgpack.loads(self.last_data, raw=USING_PYTHON_2)
else:
raise socket.timeout("timeout=" + str(self.timeout) + \
", last message time=" + str(self.last_time) + \
", current time=" + str(current_time))
def get_list(self):
""" Returns list of messages, in the order they were received"""
msg_bufer = []
try:
self.sock.settimeout(0)
while True:
self.last_data, address = self.sock.recvfrom(self.max_size)
self.last_time = monotonic()
msg = msgpack.loads(self.last_data, raw=USING_PYTHON_2)
msg_bufer.append(msg)
except socket.error:
pass
finally:
self.sock.settimeout(self.timeout)
return msg_bufer
def __del__(self):
self.sock.close()
if __name__ == "__main__":
msg = 'very important data'
a = Publisher(1000)
a.send( {"text": "magic", "number":5.5, "bool":False} )
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment