diff --git a/go_to/launch/Start.launch b/go_to/launch/Start.launch deleted file mode 100644 index f5edbad..0000000 --- a/go_to/launch/Start.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - diff --git a/go_to/src/go_to.cpp b/go_to/src/go_to.cpp deleted file mode 100644 index fcfba0c..0000000 --- a/go_to/src/go_to.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include -#include -#include -#include - -ros::Publisher cmd_vel_pub; -turtlesim::Pose current_pose; - -double target_x; -double target_y; -double distance_tolerance = 0.1; - -void poseCallback(const turtlesim::Pose::ConstPtr& msg) -{ - current_pose = *msg; - //ROS_INFO("Turtle Position -> x: [%f], y: [%f], theta: [%f]", current_pose.x, current_pose.y, current_pose.theta); -} - -void moveToGoal() -{ - geometry_msgs::Twist cmd_vel_msg; - - double K_linear = 0.5; - double K_angular = 6.0; - - float dx = target_x - current_pose.x; - float dy = target_y - current_pose.y; - - double distance = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2)); - - ROS_INFO("Current angle [%f]", current_pose.theta * 180.0 / M_PI); - - double angle_to_goal = std::atan2(dy, dx); - double angle_to_goal_degrees = angle_to_goal * 180.0 / M_PI; - ROS_INFO("Angle to goal (degrees): [%f]", angle_to_goal_degrees); - - // Check if the robot is already at the target pose - if (distance < 0.01) { - // Robot is already at the target pose, no need to move - return; - } - - if (distance >= distance_tolerance) { - cmd_vel_msg.angular.z = K_angular * (angle_to_goal - current_pose.theta); - - if ((std::abs(current_pose.theta * 180.0 / M_PI) - std::abs(angle_to_goal_degrees)) < 0.1) { - cmd_vel_msg.linear.x = K_linear * distance; - } - - //cmd_vel_msg.linear.x = K_linear * distance; - - } else { - cmd_vel_msg.linear.x = 0.0; - cmd_vel_msg.angular.z = 0.0; - ROS_INFO("Turtle reached the goal!"); - ros::shutdown(); // Optionally shut down the node after reaching the goal - } - - cmd_vel_pub.publish(cmd_vel_msg); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "go_to"); - ros::NodeHandle nh; - - nh.param("target_x", target_x, 5.0); - nh.param("target_y", target_y, 5.0); - - cmd_vel_pub = nh.advertise("/cmd_vel", 10); - ros::Subscriber pose_sub = nh.subscribe("/turtle1/pose", 10, poseCallback); - - ros::Rate loop_rate(10); // 10 Hz - - while (ros::ok()) - { - ros::spinOnce(); - moveToGoal(); - loop_rate.sleep(); - } - - return 0; -} - diff --git a/robotcraft2024_driver_g3/CMakeLists.txt b/robotcraft2024_driver_g3/CMakeLists.txt deleted file mode 100644 index 0434873..0000000 --- a/robotcraft2024_driver_g3/CMakeLists.txt +++ /dev/null @@ -1,214 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(robotcraft2024_driver_g3) - -## 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 - nav_msgs - roscpp - rospy - rosserial_arduino - rosserial_python - sensor_msgs - std_msgs - tf -) - -## 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# nav_msgs# 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 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 robotcraft2024_driver_g3 -# CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy rosserial_arduino rosserial_python sensor_msgs std_msgs tf -# 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}/robot_driver.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(robot_driver src/robot_driver.cpp) -target_link_libraries(robot_driver ${catkin_LIBRARIES}) - -add_executable(square_test src/square_test.cpp) -target_link_libraries(square_test ${catkin_LIBRARIES}) - -## 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_robotcraft2024_driver_g3.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/robotcraft2024_driver_g3/launch/robot_bringup.launch b/robotcraft2024_driver_g3/launch/robot_bringup.launch deleted file mode 100644 index 9d20c71..0000000 --- a/robotcraft2024_driver_g3/launch/robot_bringup.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/robotcraft2024_driver_g3/src/robot_driver.cpp b/robotcraft2024_driver_g3/src/robot_driver.cpp deleted file mode 100644 index ce1d320..0000000 --- a/robotcraft2024_driver_g3/src/robot_driver.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include -#include -#include -#include -#include -#include - -ros::Publisher odom_pub; -ros::Publisher ir_front_sensor_pub; -ros::Publisher ir_right_sensor_pub; -ros::Publisher ir_left_sensor_pub; - -void poseCallback(const geometry_msgs::Pose2D::ConstPtr& pose_msg) { - static tf::TransformBroadcaster br; - tf::Transform transform; - nav_msgs::Odometry odom_msg; - - odom_msg.header.stamp = ros::Time::now(); - odom_msg.header.frame_id = "odom"; - odom_msg.child_frame_id = "base_link"; - - odom_msg.pose.pose.position.x = pose_msg->x; - odom_msg.pose.pose.position.y = pose_msg->y; - odom_msg.pose.pose.orientation.z = sin(pose_msg->theta / 2.0); - odom_msg.pose.pose.orientation.w = cos(pose_msg->theta / 2.0); - - odom_pub.publish(odom_msg); - - transform.setOrigin(tf::Vector3(pose_msg->x, pose_msg->y, 0.0)); - tf::Quaternion q; - q.setRPY(0, 0, pose_msg->theta); - transform.setRotation(q); - - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link")); -} - -void frontCallback(const std_msgs::Float32::ConstPtr& front_distance_msg) { - sensor_msgs::Range range_msg; - range_msg.header.stamp = ros::Time::now(); - range_msg.header.frame_id = "front_ir"; - range_msg.radiation_type = 1; - range_msg.field_of_view = 0.034906585; - range_msg.min_range = 0.1; - range_msg.max_range = 0.8; - range_msg.range = front_distance_msg->data; - - ir_front_sensor_pub.publish(range_msg); -} - -void rightCallback(const std_msgs::Float32::ConstPtr& right_distance_msg) { - sensor_msgs::Range range_msg; - range_msg.header.stamp = ros::Time::now(); - range_msg.header.frame_id = "right_ir"; - range_msg.radiation_type = 1; - range_msg.field_of_view = 0.034906585; - range_msg.min_range = 0.1; - range_msg.max_range = 0.8; - range_msg.range = right_distance_msg->data; - - ir_right_sensor_pub.publish(range_msg); -} - -void leftCallback(const std_msgs::Float32::ConstPtr& left_distance_msg) { - sensor_msgs::Range range_msg; - range_msg.header.stamp = ros::Time::now(); - range_msg.header.frame_id = "left_ir"; - range_msg.radiation_type = 1; - range_msg.field_of_view = 0.034906585; - range_msg.min_range = 0.1; - range_msg.max_range = 0.8; - range_msg.range = left_distance_msg->data; - - ir_left_sensor_pub.publish(range_msg); -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "robot_driver"); - ros::NodeHandle nh; - - odom_pub = nh.advertise("/odom", 10); - ir_front_sensor_pub = nh.advertise("/ir_front_sensor", 10); - ir_right_sensor_pub = nh.advertise("/ir_right_sensor", 10); - ir_left_sensor_pub = nh.advertise("/ir_left_sensor", 10); - - ros::Subscriber pose_sub = nh.subscribe("/pose", 10, poseCallback); - ros::Subscriber front_distance_sub = nh.subscribe("/front_distance", 10, frontCallback); - ros::Subscriber right_distance_sub = nh.subscribe("/right_distance", 10, rightCallback); - ros::Subscriber left_distance_sub = nh.subscribe("/left_distance", 10, leftCallback); - - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/robotcraft_maze/world/540x540-90cm.png b/robotcraft_maze/world/540x540-90cm.png old mode 100755 new mode 100644 index 197b70d..d0275d1 Binary files a/robotcraft_maze/world/540x540-90cm.png and b/robotcraft_maze/world/540x540-90cm.png differ diff --git a/robotcraft_robot_description/rviz/robotcraft_robot.rviz b/robotcraft_robot_description/rviz/robotcraft_robot.rviz deleted file mode 100644 index 5d6058a..0000000 --- a/robotcraft_robot_description/rviz/robotcraft_robot.rviz +++ /dev/null @@ -1,306 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /TF1/Frames1 - - /Front IR1 - - /Odometry1 - Splitter Ratio: 0.5 - Tree Height: 787 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - acrylic_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - arduino: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - back_lf: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - battery: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bottom_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - front_ir: - Alpha: 1 - Show Axes: false - Show Trail: false - front_lf: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_ir: - Alpha: 1 - Show Axes: false - Show Trail: false - right_ir: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: true - acrylic_base: - Value: true - arduino: - Value: true - back_lf: - Value: true - base_link: - Value: true - battery: - Value: true - bottom_base: - Value: true - front_ir: - Value: true - front_lf: - Value: true - left_ir: - Value: true - odom: - Value: true - right_ir: - Value: true - Marker Alpha: 1 - Marker Scale: 0.11999999731779099 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - odom: - base_link: - acrylic_base: - {} - arduino: - {} - back_lf: - {} - battery: - {} - bottom_base: - {} - front_ir: - {} - front_lf: - {} - left_ir: - {} - right_ir: - {} - Update Interval: 0 - Value: true - - Alpha: 0.5 - Buffer Length: 1 - Class: rviz/Range - Color: 255; 255; 255 - Enabled: true - Name: Right IR - Queue Size: 100 - Topic: ir_right_sensor - Unreliable: false - Value: true - - Alpha: 0.5 - Buffer Length: 1 - Class: rviz/Range - Color: 255; 255; 255 - Enabled: true - Name: Left IR - Queue Size: 100 - Topic: ir_left_sensor - Unreliable: false - Value: true - - Alpha: 0.5 - Buffer Length: 1 - Class: rviz/Range - Color: 255; 255; 255 - Enabled: true - Name: Front IR - Queue Size: 100 - Topic: ir_front_sensor - Unreliable: false - Value: true - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 100 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Queue Size: 10 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Arrow - Topic: odom - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: odom - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/XYOrbit - Distance: 5.1602582931518555 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.07238923013210297 - Y: -0.025597572326660156 - Z: 1.4901161193847656e-08 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.1153982877731323 - Target Frame: - Yaw: 3.2067737579345703 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1016 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073b0000003efc0100000002fb0000000800540069006d006500000000000000073b0000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000005c80000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1848 - X: 72 - Y: 27 diff --git a/simstage_group3/launch/gmapping.launch b/simstage_group3/launch/gmapping.launch deleted file mode 100644 index 7f0725d..0000000 --- a/simstage_group3/launch/gmapping.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/simstage_group3/launch/reactive.launch b/simstage_group3/launch/reactive.launch deleted file mode 100644 index 9ed5617..0000000 --- a/simstage_group3/launch/reactive.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/simstage_group3/maps/rooms.jpeg b/simstage_group3/maps/rooms.jpeg index 7171f1f..718f105 100644 Binary files a/simstage_group3/maps/rooms.jpeg and b/simstage_group3/maps/rooms.jpeg differ diff --git a/simstage_group3/robots/simple_robot.inc b/simstage_group3/robots/simple_robot.inc deleted file mode 100644 index 8317515..0000000 --- a/simstage_group3/robots/simple_robot.inc +++ /dev/null @@ -1,49 +0,0 @@ -define laser ranger( - sensor( - range [0.0 5.6] - fov 240.0 - samples 240 - ) - color "blue" - size [0.06 0.06 0.06] - - block( points 4 - point[0] [0 0] - point[1] [0 1] - point[2] [1 1] - point[3] [1 0] - z [0 1] - ) -) - -define simple_robot position( - localization "odom" - odom_error [0.03 0.03 0.00 0.05] - size [0.33 0.33 0.25] - origin [0 0 0 0] - gui_nose 1 - drive "diff" - color "orange" - - block( points 16 - point[0] [ 0.225 0.000 ] - point[1] [ 0.208 0.086 ] - point[2] [ 0.159 0.159 ] - point[3] [ 0.086 0.208 ] - point[4] [ 0.000 0.225 ] - point[5] [ -0.086 0.208 ] - point[6] [ -0.159 0.159 ] - point[7] [ -0.208 0.086 ] - point[8] [ -0.225 0.000 ] - point[9] [ -0.208 -0.086 ] - point[10] [ -0.159 -0.159 ] - point[11] [ -0.086 -0.208 ] - point[12] [ -0.000 -0.225 ] - point[13] [ 0.086 -0.208 ] - point[14] [ 0.159 -0.159 ] - point[15] [ 0.208 -0.086 ] - z [0 1] - ) - - laser(pose [0.23 0.0 -0.15 0.0]) -) \ No newline at end of file diff --git a/simstage_group3/script/reactive_navigation.py b/simstage_group3/script/reactive_navigation.py deleted file mode 100755 index 072e9cc..0000000 --- a/simstage_group3/script/reactive_navigation.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/bin/python3 - -#/base_pose_ground_truth -# rostopic type : nav_msgs/Odometry -# rostopic echo : - -import rospy -from geometry_msgs.msg import Twist -from sensor_msgs.msg import LaserScan - -class ReactiveNavigation(): - def __init__(self): - self.cmd_vel = Twist() - self.laser_msg = LaserScan() - self.desired_distance_from_wall = 1.0 - self.max_distance = 3.0 - self.side = 'left' # change this to make the robot follow the choosen wall - self.obstacle_distance = 100 - self.rospy_sub_lazer = rospy.Subscriber("base_scan", LaserScan, self.laser_callback, queue_size=1) - self.pub_CMD = rospy.Publisher("cmd_vel", Twist, queue_size=1) - self.rate = rospy.Rate(5) - - def laser_callback(self, callback): - self.laser_msg = callback - - def calculate_command(self): - if type(self.laser_msg.ranges) == tuple: - side_distances = self.laser_msg.ranges - num_ranges = len(side_distances) - half_length = num_ranges / 2 - - left_side_distances = [] - right_side_distances = [] - - for i in range(num_ranges): - if i < half_length: - left_side_distances.append(side_distances[i]) - else: - right_side_distances.append(side_distances[i]) - - if len(side_distances) > 0: - self.obstacle_distance = min(side_distances) - - if self.obstacle_distance < self.desired_distance_from_wall: # To far - self.cmd_vel.linear.x = 0.5 - if self.side == 'left': - self.cmd_vel.angular.z = -1.0 - else: - self.cmd_vel.angular.z = 1.0 - elif self.obstacle_distance > self.desired_distance_from_wall: # To close - self.cmd_vel.linear.x = 0.5 - if self.side == 'left': - self.cmd_vel.angular.z = 1.0 - else: - self.cmd_vel.angular.z = -1.0 - else: # Correct distance - self.cmd_vel.linear.x = 1.0 - self.cmd_vel.angular.z = 0.0 - else: - self.cmd_vel.linear.x = 1.0 - self.cmd_vel.angular.z = 0.0 - self.pub_CMD.publish(self.cmd_vel) - - def run(self): - while not rospy.is_shutdown(): - self.calculate_command() - self.rate.sleep() - -if __name__ == '__main__': - rospy.init_node("reactive_navigation_py_group3") - controller = ReactiveNavigation() - controller.run() diff --git a/simstage_group3/worlds/rooms.world b/simstage_group3/worlds/rooms.world deleted file mode 100644 index 746c319..0000000 --- a/simstage_group3/worlds/rooms.world +++ /dev/null @@ -1,29 +0,0 @@ -define floorplan model ( - color "SlateGray" - boundary 1 - gui_nose 1 - gui_grid 1 - gui_move 1 - obstacle_return 1 -) - -window( - size [700 650 1] - rotate [ 0 0] - center [ 13.325 10.825 0 ] - scale 25 - show_data 1 - show_clock 1 -) - -floorplan ( - size [37.05 31.65 1.000] - pose [13.525 10.825 0.000 0.000] - bitmap "../maps/example.pgm" -) - -#include "../robots/simple_robot.inc" -#simple_robot(pose [ 8.1 7.1 0.000 91.000 ] name "robo") - -include "../robots/wall-e.inc" -walle(pose [ 6.1 6.1 0.000 91.000 ] name "robot_0") \ No newline at end of file diff --git a/stage_tutorial/CMakeLists.txt b/stage_tutorial/CMakeLists.txt deleted file mode 100644 index ff5469d..0000000 --- a/stage_tutorial/CMakeLists.txt +++ /dev/null @@ -1,213 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(stage_tutorial) - -## 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 - nav_msgs - roscpp - rospy - sensor_msgs - stage_ros - std_msgs - tf -) - -## 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# nav_msgs# 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 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 stage_tutorial -# CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy sensor_msgs stage_ros std_msgs tf -# 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}/stage_tutorial.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/stage_tutorial_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(reactive_navigation src/reactive_navigation.cpp) -#target_link_libraries(reactive_navigation ${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_stage_tutorial.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/stage_tutorial/maps/rooms.png b/stage_tutorial/maps/rooms.png old mode 100755 new mode 100644 index 8e466c7..37049f5 Binary files a/stage_tutorial/maps/rooms.png and b/stage_tutorial/maps/rooms.png differ diff --git a/stage_tutorial/script/reactive_navigation.py b/stage_tutorial/script/reactive_navigation.py deleted file mode 100755 index 5245aeb..0000000 --- a/stage_tutorial/script/reactive_navigation.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/bin/python3 - -# chmod +x -import rospy -from geometry_msgs.msg import Twist -from sensor_msgs.msg import LaserScan - -class ReactiveNavigation(): - def __init__(self): - self.cmd_vel = Twist() - - self.laser_msg_0 = LaserScan() - self.laser_msg_1 = LaserScan() - self.laser_msg_2 = LaserScan() - self.laser_msg_3 = LaserScan() - - self.robot_stopped = False - self.obstacle_distance = 100 - - self.active_robot = str(rospy.get_param("reactive_controller_py/active_robot")) - self.available_robots = list(rospy.get_param("reactive_controller_py/available_robots")) - - self._cmd_topic = self.active_robot + "/cmd_vel" - self._laser_topic = self.active_robot + "/base_scan" - - self.rate = rospy.Rate(5) - - - def laser_bc_0(self, callback): - self.laser_msg_0 = callback - def laser_bc_1(self, callback): - self.laser_msg_1 = callback - def laser_bc_2(self, callback): - self.laser_msg_2 = callback - def laser_bc_3(self, callback): - self.laser_msg_3 = callback - - def calculate_command(self): - for robot in self.available_robots: - if robot == "robot_0": - self.publisher_robot(robot, self.laser_msg_0) - elif robot == "robot_1": - self.publisher_robot(robot, self.laser_msg_1) - elif robot == "robot_2": - self.publisher_robot(robot, self.laser_msg_2) - elif robot == "robot_3": - self.publisher_robot(robot, self.laser_msg_3) - - def run(self): - while not rospy.is_shutdown(): - self.calculate_command() - self.rate.sleep() - - def publisher_robot(self, robot, laser_msg): - cmd_vel = Twist() - _cmd_topic = robot + "/cmd_vel" - _laser_topic = robot + "/base_scan" - - if robot == "robot_0": - rospy.Subscriber(_laser_topic, LaserScan, self.laser_bc_0, queue_size=1) - elif robot == "robot_1": - rospy.Subscriber(_laser_topic, LaserScan, self.laser_bc_1, queue_size=1) - elif robot == "robot_2": - rospy.Subscriber(_laser_topic, LaserScan, self.laser_bc_2, queue_size=1) - elif robot == "robot_3": - rospy.Subscriber(_laser_topic, LaserScan, self.laser_bc_3, queue_size=1) - - pub_CMD = rospy.Publisher(_cmd_topic, Twist, queue_size=1) - - if type(laser_msg.ranges) == tuple: - obstacle_distance = min(laser_msg.ranges) - - if obstacle_distance > 1.0: - cmd_vel.linear.x = 1.0 - cmd_vel.angular.z = 0.0 - else: - cmd_vel.linear.x = 0.0 - cmd_vel.angular.z = 1.0 - - #obstacle_distance = 100 - - pub_CMD.publish(cmd_vel) - - - -if __name__ == '__main__': - rospy.init_node('reactive_navigation_py') - controller = ReactiveNavigation() - controller.run() \ No newline at end of file diff --git a/stage_tutorial/worlds/rooms.world b/stage_tutorial/worlds/rooms.world deleted file mode 100644 index 15091bb..0000000 --- a/stage_tutorial/worlds/rooms.world +++ /dev/null @@ -1,30 +0,0 @@ -include "../robots/simple_robot.inc" -simple_robot(pose [ 10.1 9.1 0.000 91.000 ] name "robo1") - - -define floorplan model ( - color "SlateGray" - boundary 1 - gui_nose 1 - gui_grid 1 - gui_move 1 - obstacle_return 1 -) - -window( - size [700 650 1] - rotate [ 0 0] - center [ 13.325 10.825 0 ] - scale 25 - show_data 1 - show_clock 1 -) - -floorplan ( - size [27.05 21.65 1.000] - pose [13.525 10.825 0.000 0.000] - bitmap "../maps/rooms.png" -) - -#include "../robots/simple_robots.png" -#simple_robot (pose [ 11.1 9.2 0.000 90.000] name "robot")