From 6d01be11685e721b256dc139f6e04b8b5427f535 Mon Sep 17 00:00:00 2001 From: Drew Bednar Date: Thu, 3 Mar 2022 04:19:45 -0800 Subject: [PATCH] Initial commit --- CMakeLists.txt | 205 +++++++++++++++++++++++++ package.xml | 65 ++++++++ scripts/tuturalsim_motion_pose.py | 241 ++++++++++++++++++++++++++++++ 3 files changed, 511 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 package.xml create mode 100755 scripts/tuturalsim_motion_pose.py diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f4f90e4 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 3.0.2) +project(turtle_sim_nav) + +## 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 + geometry_msgs + std_msgs +) + +## 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 +# 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 +# geometry_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 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 turtle_sim_nav +# CATKIN_DEPENDS geometry_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 + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/turtle_sim_nav.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/turtle_sim_nav_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/my_python_script +# 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 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_turtle_sim_nav.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) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..4814122 --- /dev/null +++ b/package.xml @@ -0,0 +1,65 @@ + + + turtle_sim_nav + 0.0.0 + The turtle_sim_nav package + + + + + toor + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + std_msgs + geometry_msgs + std_msgs + geometry_msgs + std_msgs + + + + + + + + diff --git a/scripts/tuturalsim_motion_pose.py b/scripts/tuturalsim_motion_pose.py new file mode 100755 index 0000000..a73306a --- /dev/null +++ b/scripts/tuturalsim_motion_pose.py @@ -0,0 +1,241 @@ +#!/usr/bin/env python3 +import math +import time + +import rospy +from geometry_msgs.msg import Twist +from turtlesim.msg import Pose + + +# Global State +x = 0 +y = 0 +yaw = 0 + +def pose_callback(pose_msg): + global x, y, yaw + x = pose_msg.x + y = pose_msg.y + yaw = pose_msg.theta + +# TODO CONVERT ALL OF THIS INTO A CLASS +# TODO ADD TYPE ANNOTATIONS TO ALL THIS SHIT + +def grid_clean(velocity_publisher): + + desired_pose = Pose() + desired_pose.x = 1 + desired_pose.y = 1 + desired_pose.theta = 0 + + go_to_goal(velocity_publisher, 1, 1) + + set_desired_orientation(velocity_publisher, 30, math.radians(desired_pose.theta)) + + for i in range(5): + move(velocity_publisher, 2.0, 1.0, True) + rotate(velocity_publisher, 20, 90, False) + move(velocity_publisher, 2.0, 9.0, True) + rotate(velocity_publisher, 20, 90, True) + move(velocity_publisher, 2.0, 1.0, True) + rotate(velocity_publisher, 20, 90, True) + move(velocity_publisher, 2.0, 9.0, True) + rotate(velocity_publisher, 20, 90, False) + pass + +def spiral(velocity_publisher, rk, wk): + """ + r = a + b(Theta) + """ + velocity_msg = Twist() + loop_rate = rospy.Rate(1) + + while (x< 10.5) and (y < 10.5): + # At every iteration we will increase the linear velocity + # We keep constant the angular velocity though + rk = rk+1 + velocity_msg.linear.x = rk + velocity_msg.angular.z = wk + + velocity_publisher.publish(velocity_msg) + loop_rate.sleep() + + velocity_msg.linear.x = 0 + velocity_msg.angular.z = 0 + velocity_publisher.publish(velocity_msg) + +def set_desired_orientation(velocity_publisher, speed_in_degrees, desired_angle_degrees): + """Set desired orientation as an absolute angle.""" + relative_angle_radians = math.radians(desired_angle_degrees) - yaw + clockwise = 0 + if relative_angle_radians < 0: + clockwise = 1 + else: + closewise = 0 + print("relative_angle_radians: ", math.degrees(relative_angle_radians)) + print("desired_angle_degree: ", desired_angle_degrees) + rotate(velocity_publisher, speed_in_degrees, math.degrees(abs(relative_angle_radians)), clockwise) + + + +# TODO YOu could update this to accept a Pose message as the +# Target, with a distance tolerance (Epsilon) used when determining +# if you have arrived at the target +def go_to_goal(velocity_publisher, x_goal, y_goal): + global x, y, yaw + + velocity_message = Twist() + + while True: + # TODO Make this a param + # aka Kp proportional gain for linear velocity + K_linear = 0.5 + distance = abs(math.sqrt(((x_goal-x) ** 2) + ((y_goal-y) ** 2))) + + # This is the P in PID Control. So we have P-Controller + # The linear speed is proportional to the distance + # K_linear is our contanst. + # So if the distance is high, the speed will be high, + # but if the distance is 0 the speed will be zero + linear_speed = distance * K_linear + + # TODO Make this a param + # or Ki which is the proporional gain for the angular velocity + K_angular = 4.0 + # Mathematically, atan2 gives the angle between two vectors + desired_angle_goal = math.atan2(y_goal-y, x_goal-x) + + # Likewise the angular speed is proportional to the distance + # beween the desired angle and the current angle + # The larger the angle the larger the angular speed + angular_speed = (desired_angle_goal-yaw)*K_angular + + velocity_message.linear.x = linear_speed + velocity_message.angular.z = angular_speed + + velocity_publisher.publish(velocity_message) + print(f"x={x}, y={y}, distance to goal: {distance} ") + + # With float values it is extremely hard to comare a + # distance == 0 so we want to use some kind of reasoble + # error aka epsilon + if distance < 0.01: + break + + +def rotate(velocity_publisher, angular_speed_degree, relative_angle_degree, clockwise): + """ + velocity_publisher: Publisher + angular_speed_degree: Degrees per second + relative_ + """ + velocity_message = Twist() + + # Convert to radians + angular_speed = math.radians(abs(angular_speed_degree)) + + if clockwise: + velocity_message.angular.z = -abs(angular_speed) + else: + velocity_message.angular.z = abs(angular_speed) + + loop_rate = rospy.Rate(10) + t0 = rospy.Time.now().to_sec() + + while True: + rospy.loginfo("Turtlesim rotates") + velocity_publisher.publish(velocity_message) + + # Here we are calculating the rotated angle over time + t1 = rospy.Time.now().to_sec() + # This is an estimate + current_angle_degree = (t1-t0) * angular_speed_degree + loop_rate.sleep() # In CPP you would also need to call ros spinOnce to dispatch one message + + if current_angle_degree > relative_angle_degree: + rospy.loginfo("Reached") + break + + velocity_message.angular.z = 0 + velocity_publisher.publish(velocity_message) + +def move(velocity_publisher, speed, distance, is_forward): + velocity_msg = Twist() + + # I don't like the global state access personally... + global x, y + + # Save initial location + x0 = x + y0 = y + + if is_forward: + velocity_msg.linear.x = abs(speed) + else: + velocity_msg.linear.x = -abs(speed) + + distance_moved = 0.0 + + # NOTE if you have a lower update rate and higher velocity the + # Robot will move further than intended since it is not getting + # timely updates on if it has reached it's goal or not. + # SO the effect of the loop rate has significant impact on the postion + # achieved by the robot. + # + #[INFO] [1646232853.396795]: Turtlesim moves forward + # 4.0959999561309814 + # We can see that the target of 4.0 was exceeded by 0.095~ meters + # a higher refresh rate would have detected that the target distance + # was reached sooner. + loop_rate = rospy.Rate(10) # Publish velocity message 10 times a second + + while True: + rospy.loginfo("Turtlesim moves forward") + velocity_publisher.publish(velocity_msg) + loop_rate.sleep() + # Calc Distance + # Linear distance formula. (2 dimensions) + # This works because the global x and y state are updated continually by the Pose callback + # function that reports the robot's state. + # + # Alternative we could have taken the speed multipled by the difference in time (speed * (t1 - t0)) + # which would have needed us to grab a t0 and t1 using time.time() since that returns a time in seconds + # This would not have required you to subscribe to the pose of the robot so there wouldn't be any global + # state to manage + # + # THE ROS WAY TO CORRECTLY CALCULATE DISTANCE WOULD HAVE BEEN TO USE tf AKA TRANSFORMS! + # That is covered in the SLAM & Nav in ROS for begginers 2 + distance_moved = abs(math.sqrt(((x - x0) **2) + ((y-y0) ** 2))) + print(distance_moved) + if distance_moved > distance: + rospy.loginfo("Reached") + break + + #Stop the robot when the target distance has been reached + velocity_msg.linear.x = 0 + velocity_publisher.publish(velocity_msg) + + +# TODO Create an action for requesting a move. + +if __name__ == "__main__": + try: + rospy.init_node("tuturalsim_motion_pose", anonymous=True) + + #declare velocity publisher + cmd_vel_topic = '/turtle1/cmd_vel' + velocity_publisher = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10) + + postion_topic = "/turtle1/pose" + pose_subscriber = rospy.Subscriber(postion_topic, Pose, pose_callback) + time.sleep(2) + # Move one meter per second traveling forward for a target distance of 4 meters + # move(velocity_publisher, 1.0, 4.0, True) + # # Rotate 5 degrees a second for 45 degrees + # rotate(velocity_publisher, 5.0, 45, False) + # go_to_goal(velocity_publisher, 2 ,2) + # set_desired_orientation(velocity_publisher, 30, 45) + # spiral(velocity_publisher, 0, 2) + grid_clean(velocity_publisher) + except rospy.ROSInterruptException: + rospy.loginfo("Node Terminated") \ No newline at end of file