diff --git a/ch5_pkg/launch/ch5_3.launch b/ch5_pkg/launch/ch5_3.launch index cb30d34..95821e7 100644 --- a/ch5_pkg/launch/ch5_3.launch +++ b/ch5_pkg/launch/ch5_3.launch @@ -3,7 +3,7 @@ -   + diff --git a/ch7_pkg/CMakeLists.txt b/ch7_pkg/CMakeLists.txt new file mode 100644 index 0000000..915851b --- /dev/null +++ b/ch7_pkg/CMakeLists.txt @@ -0,0 +1,207 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ch7_pkg) + +## 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 + 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 +# 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 ch7_pkg +# CATKIN_DEPENDS roscpp rospy 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}/ch7_pkg.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/ch7_pkg_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_ch7_pkg.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/ch7_pkg/launch/ch7_1.launch b/ch7_pkg/launch/ch7_1.launch new file mode 100644 index 0000000..e927de1 --- /dev/null +++ b/ch7_pkg/launch/ch7_1.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ch7_pkg/map.pgm b/ch7_pkg/map.pgm new file mode 100644 index 0000000..4663137 --- /dev/null +++ b/ch7_pkg/map.pgm @@ -0,0 +1,5 @@ +P5 +# CREATOR: map_saver.cpp 0.050 m/pix +384 384 +255 + \ No newline at end of file diff --git a/ch7_pkg/map.yaml b/ch7_pkg/map.yaml new file mode 100644 index 0000000..6ad3381 --- /dev/null +++ b/ch7_pkg/map.yaml @@ -0,0 +1,7 @@ +image: map.pgm +resolution: 0.050000 +origin: [-10.000000, -10.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/ch7_pkg/mymap.pgm b/ch7_pkg/mymap.pgm new file mode 100644 index 0000000..0d675c5 --- /dev/null +++ b/ch7_pkg/mymap.pgm @@ -0,0 +1,5 @@ +P5 +# CREATOR: map_saver.cpp 0.050 m/pix +384 384 +255 + \ No newline at end of file diff --git a/ch7_pkg/mymap.yaml b/ch7_pkg/mymap.yaml new file mode 100644 index 0000000..0b1cfac --- /dev/null +++ b/ch7_pkg/mymap.yaml @@ -0,0 +1,7 @@ +image: mymap.pgm +resolution: 0.050000 +origin: [-10.000000, -10.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/ch7_pkg/package.xml b/ch7_pkg/package.xml new file mode 100644 index 0000000..5123358 --- /dev/null +++ b/ch7_pkg/package.xml @@ -0,0 +1,71 @@ + + + ch7_pkg + 0.0.0 + The ch7_pkg package + + + + + joy + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + tf + roscpp + rospy + std_msgs + tf + roscpp + rospy + std_msgs + tf + + + + + + + + diff --git a/my_turtle/src/MoveTurtle.cpp b/my_turtle/src/MoveTurtle.cpp index 34ddeb1..79b660f 100644 --- a/my_turtle/src/MoveTurtle.cpp +++ b/my_turtle/src/MoveTurtle.cpp @@ -1,119 +1,202 @@ #include "ros/ros.h" #include "geometry_msgs/Twist.h" #include "turtlesim/Pose.h" +#include +#include -double pos_x; -double pos_y; -double theta; -// Topic messages callback +// Define a simple structure for a 2D point (waypoint) +struct Point { + double x; + double y; +}; + +// Global variables to store the turtle's current pose +double pos_x = 0.0; +double pos_y = 0.0; +double theta = 0.0; + +// Waypoints definition: A sequence of (x, y) points for the turtle to follow +std::vector waypoints = { + {5.0, 1.0}, + {1.0, 5.0}, + {5.0, 9.0}, + {9.0, 5.0}, + {5.0, 1.0}, + {1.0, 5.0}, + {5.0, 9.0}, + {9.0, 5.0}, + {5.0, 1.0}, + {1.0, 5.0}, + {5.0, 9.0}, + {9.0, 5.0}, + {5.0, 1.0}, + {1.0, 5.0}, + {5.0, 9.0}, + {9.0, 5.0}, + {5.0, 1.0}, + {1.0, 5.0}, + {5.0, 9.0}, + {9.0, 5.0}, + {5.0, 1.0}, + {1.0, 5.0}, + {5.0, 9.0}, + {9.0, 5.0}, + {5.0, 1.0}, + {1.0, 5.0}, + {5.0, 9.0}, + {9.0, 5.0}, + {5.0, 1.0}, + {1.0, 5.0}, + {5.0, 9.0}, + {9.0, 5.0}, + {5.0, 1.0}, + {1.0, 5.0}, + {5.0, 9.0}, + {9.0, 5.0} + + // {5.54, 5.54} // Center +}; + +// Control parameters +const double ANGULAR_SPEED_GAIN = 2.0; +const double LINEAR_SPEED = 2.0; +const double DISTANCE_THRESHOLD = 0.1; +const double ANGLE_THRESHOLD_RAD = 0.01; +// New parameter: Time to pause at each waypoint +const double WAIT_TIME_SECONDS = 0.0; + +// State variables for waypoint tracking +int current_target_index = 0; +double target_x = waypoints[0].x; +double target_y = waypoints[0].y; + +// --- State Machine --- +enum State { + NAVIGATE, // Actively moving or turning towards the target + WAITING, // Paused at the target waypoint + FINISHED // All waypoints reached +}; + +State current_state = NAVIGATE; +ros::Time wait_start_time; // To track when the waiting period began + +// --- Callback Function --- void poseCallback(const turtlesim::PoseConstPtr& msg) { - pos_x=msg->x; - pos_y=msg->y; - theta=msg->theta; - // ROS_INFO("x: %.2f, y: %.2f, theta: %.2f", msg->x, msg->y, msg->theta); + pos_x = msg->x; + pos_y = msg->y; + theta = msg->theta; } +// Function to normalize an angle to the range [-pi, pi] +double normalizeAngle(double angle) +{ + while (angle > M_PI) angle -= 2.0 * M_PI; + while (angle < -M_PI) angle += 2.0 * M_PI; + return angle; +} + +// --- Main Program --- int main(int argc, char **argv) { - const double FORWARD_SPEED_MPS = 0; - - // Initialize the node - ros::init(argc, argv, "move_turtle"); + // Initialize the ROS node + ros::init(argc, argv, "move_turtle_waypoints"); ros::NodeHandle node; - // A publisher for the movement data + // Publisher and Subscriber setup ros::Publisher pub = node.advertise("turtle1/cmd_vel", 10); - - // A listener for pose ros::Subscriber sub = node.subscribe("turtle1/pose", 10, poseCallback); - // Drive forward at a given speed. The robot points up the x-axis. - // The default constructor will set all commands to 0 - geometry_msgs::Twist msg; - msg.linear.x = FORWARD_SPEED_MPS; - - // Loop at 10Hz, publishing movement commands until we shut down - ros::Rate rate(10); - ROS_INFO("Starting to move forward"); - int flag1=0; - int flag2=0; - int flag3=0; - int init =1; - int p0 = 0; - int p1 =0; - int p2=0; - while (ros::ok()) { - - if(flag1 == 0){ - if(theta < 6.25/4){ - msg.angular.z=1; - }else{ - if(init == 1){ - ROS_INFO("x: %.2f, y: %.2f, theta: %.2f", pos_x,pos_y,theta); - init = 0; - } - msg.angular.z=0; - msg.linear.x=1; - flag1=1; - flag3=0; - } - }else{ - if(pos_y > 10){ - if(p0==0){ - ROS_INFO("x: %.2f, y: %.2f, theta: %.2f", pos_x,pos_y,theta); - p0=1; - } - msg.linear.x=0; - flag2 = 2; - } - } - - - - if(flag2 == 2){ - if(theta > -0.6 || theta > 0){ - msg.angular.z=-1; - }else{ - - flag2 = 1; - msg.angular.z=0; - msg.linear.x=1; - } - }else if(flag1 == 1 && flag2 == 1){ - if(pos_y < 5){ - if(p1 == 0){ - ROS_INFO("x: %.2f, y: %.2f, theta: %.2f", pos_x,pos_y,theta); - p1=1; - } - flag3 =3; - flag2=0; - msg.angular.z=-1; - msg.linear.x=0; - } - } - if(flag3==3){ - if(theta > 3){ - - msg.angular.z=0; - msg.linear.x=1; - flag3=5; - } - - } - - if(flag3==5){ - if(pos_x < 5){ - if(p2==0){ - ROS_INFO("x: %.2f, y: %.2f, theta: %.2f", pos_x,pos_y,theta); - p2=1; - } - - msg.linear.x=0; - } - } + // Set the loop rate + ros::Rate rate(10); + ROS_INFO("Starting waypoint navigation with %zu points. Pausing for %.1f seconds at each.", + waypoints.size(), WAIT_TIME_SECONDS); + + // Main control loop + while (ros::ok()) { + geometry_msgs::Twist msg; // Command message, all fields set to 0 initially + + switch (current_state) { + case NAVIGATE: { + // 1. Calculate the distance and angle to the current target + double dx = target_x - pos_x; + double dy = target_y - pos_y; + double distance = std::sqrt(dx * dx + dy * dy); + + // 2. Check if the target is reached + if (distance < DISTANCE_THRESHOLD) { + ROS_INFO("Waypoint %d reached: (%.2f, %.2f). Starting %.1fs wait.", + current_target_index + 1, target_x, target_y, WAIT_TIME_SECONDS); + + // Transition to the WAITING state + current_state = WAITING; + wait_start_time = ros::Time::now(); + msg.linear.x = 0.0; + msg.angular.z = 0.0; + break; // Move to the next state check immediately + } + + // 3. Navigation logic (Turn and Move) + double target_angle = std::atan2(dy, dx); + double angle_error = normalizeAngle(target_angle - theta); + + msg.angular.z = ANGULAR_SPEED_GAIN * angle_error; + + if (std::abs(angle_error) < ANGLE_THRESHOLD_RAD) { + // Facing target, move forward + msg.linear.x = LINEAR_SPEED; + msg.angular.z = 0.0; + } else { + // Not facing target, just turn + msg.linear.x = 0.0; + } + break; + } + + case WAITING: { + // 1. Remain stopped (msg already 0.0 from previous step) + + // 2. Check if the wait time is over + if ((ros::Time::now() - wait_start_time).toSec() >= WAIT_TIME_SECONDS) { + + // 3. Advance to the next waypoint + current_target_index++; + + if (current_target_index < waypoints.size()) { + // There are more points, set the new target + target_x = waypoints[current_target_index].x; + target_y = waypoints[current_target_index].y; + ROS_INFO("Resuming navigation to next point: (%2.f, %2.f)", target_x, target_y); + current_state = NAVIGATE; // Transition back to NAVIGATE + } else { + // All points complete + ROS_INFO("All waypoints have been processed. Task finished."); + current_state = FINISHED; // Transition to FINISHED + } + } + break; + } + + case FINISHED: { + // Keep publishing zero velocity to ensure it stays stopped + msg.linear.x = 0.0; + msg.angular.z = 0.0; + break; + } + } + + // Publish the movement commands pub.publish(msg); - ros::spinOnce(); // Allow processing of incoming messages + + // Allow ROS to process events + ros::spinOnce(); rate.sleep(); + + // Exit the loop if finished + if (current_state == FINISHED) { + break; + } } + + return 0; } \ No newline at end of file diff --git a/pluginlib_11463144/CMakeLists.txt b/pluginlib_11463144/CMakeLists.txt new file mode 100644 index 0000000..6f11c90 --- /dev/null +++ b/pluginlib_11463144/CMakeLists.txt @@ -0,0 +1,214 @@ +cmake_minimum_required(VERSION 3.0.2) +project(pluginlib_11463144) + +## 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 + pluginlib + roscpp +) + +## 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 +# std_msgs # Or other packages containing 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 pluginlib_11463144 +# CATKIN_DEPENDS pluginlib roscpp +# 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} +) + +add_library(polygon_plugins src/polygon_plugins.cpp) +add_executable(polygon_loader src/polygon_loader.cpp) + +# Link the executable to roscpp and pluginlib +target_link_libraries(polygon_loader ${catkin_LIBRARIES}) + +# ⭐ ADD THIS LINE: Link your plugin library to roscpp and pluginlib +# This is required for PLUGINLIB_EXPORT_CLASS to work. +target_link_libraries(polygon_plugins ${catkin_LIBRARIES}) +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/pluginlib_11463144.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/pluginlib_11463144_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_pluginlib_11463144.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/pluginlib_11463144/include/pluginlib_11463144/polygon_base.h b/pluginlib_11463144/include/pluginlib_11463144/polygon_base.h new file mode 100644 index 0000000..64914d7 --- /dev/null +++ b/pluginlib_11463144/include/pluginlib_11463144/polygon_base.h @@ -0,0 +1,15 @@ +#ifndef PLUGINLIB_TUTORIALS__POLYGON_BASE_H_ +#define PLUGINLIB_TUTORIALS__POLYGON_BASE_H_ +namespace polygon_base +{ + class RegularPolygon + { + public: + virtual void initialize(double side_length) = 0; + virtual double area() = 0; + virtual ~RegularPolygon(){} + protected: + RegularPolygon(){} + }; +}; +#endif diff --git a/pluginlib_11463144/include/pluginlib_11463144/polygon_plugins.h b/pluginlib_11463144/include/pluginlib_11463144/polygon_plugins.h new file mode 100644 index 0000000..1a6bb41 --- /dev/null +++ b/pluginlib_11463144/include/pluginlib_11463144/polygon_plugins.h @@ -0,0 +1,198 @@ +#ifndef PLUGINLIB_TUTORIALS__POLYGON_PLUGINS_H_ +#define PLUGINLIB_TUTORIALS__POLYGON_PLUGINS_H_ +#include // 假設你的路徑是這個，ROS 教學中通常是 +#include //cmath標頭檔為特殊運算語法， + //沒有寫的話無法執行pow(,)平方運算語法 +#include // 用於拋出例外 + +namespace polygon_plugins +{ + // --- 3 邊形 (Triangle) --- + class Triangle : public polygon_base::RegularPolygon + { + public: + Triangle(){} + + void initialize(double side_length) + { + side_length_ = side_length; + } + + double area() + { + return 0.5 * side_length_ * getHeight(); + } + + double getHeight() + { + // 等邊三角形的高: h = sqrt(a^2 - (a/2)^2) = sqrt(3)/2 * a + return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2))); + } + + private: + double side_length_; + }; + + // --- 4 邊形 (Square) --- + class Square : public polygon_base::RegularPolygon + { + public: + Square(){} + + void initialize(double side_length) + { + side_length_ = side_length; + } + + double area() + { + return side_length_ * side_length_; + } + + private: + double side_length_; + + }; + + // --- N 邊形 (N-Sided Polygon) (通用實現) --- + // 為了處理 5 到 10 邊，我們實現一個通用的 RegularPolygon 類， + // 這樣就不需要為每個邊數都寫一個類。 + class RegularPolygonN : public polygon_base::RegularPolygon +{ + public: + // 修正: 構造函數現在可以正確初始化成員變數 + RegularPolygonN() : num_sides_(0), side_length_(0.0) {} + + // 修正: 實現基類的純虛擬函數 initialize + void initialize(double side_length) override // 使用 override 確保正確覆寫 + { + side_length_ = side_length; + // ⚠️ 注意：由於 initialize 只接受邊長，num_sides_ 必須在外部設定或在這個類中固定 + // 我們將假設這個類將被用於一個固定的 N (例如 Octagon) + // 若 num_sides_ 為 0，area() 將會返回 0。 + } + + // 新增 setter (非 pluginlib 標準，用於外部調試或測試) + void set_num_sides(int n) + { + if (n >= 3) num_sides_ = n; + } + inline double calculate_polygon_area(int n, double s) + { + if (n < 3) return 0.0; + return (n * s * s) / (4.0 * tan(M_PI / n)); + } + // 修正: 實現基類的純虛擬函數 area + double area() override + { + if (num_sides_ > 10) + { + // 當 N > 10 時，視為圓形，side_length_ 為半徑 + double radius = side_length_; + return M_PI * radius * radius; + } + else if (num_sides_ >= 3 && num_sides_ <= 10) + { + // N = 3 到 10 時，計算多邊形面積 + return calculate_polygon_area(num_sides_, side_length_); + } + else + { + return 0.0; + } + } + + private: + // ⭐ 修正: 新增 side_length_ 和 num_sides_ 的宣告 + double side_length_; + int num_sides_; +}; + + // --- 5 邊形 (Pentagon) 到 10 邊形 (Decagon) --- + // 使用通用面積公式：Area = (n * s^2) / (4 * tan(pi/n)) + + // 協助計算面積的通用函數 + inline double calculate_area(int n, double s) + { + // M_PI 是 cmath (或 math.h) 中定義的 π 值 + double apothem = s / (2.0 * tan(M_PI / n)); // 邊心距 + return (0.5 * n * s) * apothem; // 面積 = 1/2 * 周長 * 邊心距 + } + + + class Pentagon : public polygon_base::RegularPolygon + { + public: + Pentagon(){} + void initialize(double side_length) { side_length_ = side_length; } + double area() { return calculate_area(5, side_length_); } + private: + double side_length_; + }; + + class Hexagon : public polygon_base::RegularPolygon + { + public: + Hexagon(){} + void initialize(double side_length) { side_length_ = side_length; } + double area() { return calculate_area(6, side_length_); } + private: + double side_length_; + }; + + class Heptagon : public polygon_base::RegularPolygon + { + public: + Heptagon(){} + void initialize(double side_length) { side_length_ = side_length; } + double area() { return calculate_area(7, side_length_); } + private: + double side_length_; + }; + + class Octagon : public polygon_base::RegularPolygon + { + public: + Octagon(){} + void initialize(double side_length) { side_length_ = side_length; } + double area() { return calculate_area(8, side_length_); } + private: + double side_length_; + }; + + class Nonagon : public polygon_base::RegularPolygon + { + public: + Nonagon(){} + void initialize(double side_length) { side_length_ = side_length; } + double area() { return calculate_area(9, side_length_); } + private: + double side_length_; + }; + + class Decagon : public polygon_base::RegularPolygon + { + public: + Decagon(){} + void initialize(double side_length) { side_length_ = side_length; } + double area() { return calculate_area(10, side_length_); } + private: + double side_length_; + }; + class Circle : public polygon_base::RegularPolygon + { + public: + Circle(){} + void initialize(double radius) { radius_ = radius; } + double area() { return M_PI * radius_ * radius_; } + private: + double radius_; + }; +}; // end namespace polygon_plugins + +// 確保 M_PI 在某些環境下可用 +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#endif // PLUGINLIB_TUTORIALS__POLYGON_PLUGINS_H_ \ No newline at end of file diff --git a/pluginlib_11463144/package.xml b/pluginlib_11463144/package.xml new file mode 100644 index 0000000..ea608b3 --- /dev/null +++ b/pluginlib_11463144/package.xml @@ -0,0 +1,64 @@ + + + pluginlib_11463144 + 0.0.0 + The pluginlib_11463144 package + + + + + joy + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + pluginlib + roscpp + pluginlib + roscpp + pluginlib + roscpp + + + + + + + diff --git a/pluginlib_11463144/polygon_plugins.xml b/pluginlib_11463144/polygon_plugins.xml new file mode 100644 index 0000000..ead9b99 --- /dev/null +++ b/pluginlib_11463144/polygon_plugins.xml @@ -0,0 +1,33 @@ + + + + A regular triangle plugin. + + + A regular square plugin. + + + + Circle approximation plugin. + + + + A regular pentagon plugin. + + + A regular hexagon plugin. + + + A regular heptagon plugin. + + + A regular octagon plugin. + + + A regular nonagon plugin. + + + A regular decagon plugin. + + + \ No newline at end of file diff --git a/pluginlib_11463144/src/polygon_loader.cpp b/pluginlib_11463144/src/polygon_loader.cpp new file mode 100644 index 0000000..2e80254 --- /dev/null +++ b/pluginlib_11463144/src/polygon_loader.cpp @@ -0,0 +1,111 @@ +#include +#include // 替換為你的實際路徑 +#include // 引入 ROS/ROS_INFO +#include +#include +#include // 用於 std::atoi 和 std::stod + +int main(int argc, char** argv) +{ + // 1. 檢查參數數量 + if (argc != 3) + { + ROS_ERROR("Usage: polygon_plugin_tester "); + return 1; + } + + // 2. 解析參數 + int num_sides = 0; + double length = 0.0; // 可能是邊長 S 或半徑 R + std::string class_name; + + try + { + // argv[1] 是邊數 N + num_sides = std::atoi(argv[1]); + + // argv[2] 是邊長 S 或半徑 R + length = std::stod(argv[2]); + } + catch (const std::exception& e) + { + ROS_ERROR("Invalid input for number of sides or length: %s", e.what()); + return 1; + } + + // 3. 根據 N 動態選擇插件類名 + if (num_sides <= 2) + { + ROS_ERROR("A polygon must have at least 3 sides. Input N=%d is invalid.", num_sides); + return 1; + } + else if (num_sides == 3) + { + class_name = "polygon_plugins::Triangle"; + } + else if (num_sides == 4) + { + class_name = "polygon_plugins::Square"; + } + // 處理 5 到 10 邊的標準多邊形 + else if (num_sides >= 5 && num_sides <= 10) + { + // ⭐ FIX: Load the specific classes you defined in polygon_plugins.h + switch (num_sides) + { + case 5: class_name = "polygon_plugins::Pentagon"; break; + case 6: class_name = "polygon_plugins::Hexagon"; break; + case 7: class_name = "polygon_plugins::Heptagon"; break; + case 8: class_name = "polygon_plugins::Octagon"; break; + case 9: class_name = "polygon_plugins::Nonagon"; break; + case 10: class_name = "polygon_plugins::Decagon"; break; + default: break; + } + } + // 核心邏輯：如果邊數超過 10 邊，則視為圓形 + else // num_sides > 10 + { + class_name = "polygon_plugins::Circle"; + ROS_WARN("Number of sides (%d) is over 10. Calculating area as a circle (Radius = %.2f).", num_sides, length); + } + + // 初始化 ROS (用於 ROS_INFO 和 ROS_ERROR) + ros::init(argc, argv, "polygon_plugin_tester", ros::init_options::AnonymousName); + + // 4. 加載插件並計算 + // ⭐ FIX: Change "pluginlib_tutorials_" to your actual package name + pluginlib::ClassLoader poly_loader( + "pluginlib_11463144", + "polygon_base::RegularPolygon" + ); + + boost::shared_ptr polygon; + + try + { + // 加載動態決定的類 + polygon = poly_loader.createInstance(class_name); + + // 初始化邊長 (對於圓形來說就是半徑) + polygon->initialize(length); + + double area = polygon->area(); + + // 輸出結果 + if (num_sides > 10) { + ROS_INFO("Shape: Circle (N=%d, R=%.2f). Area: %.4f", num_sides, length, area); + } else { + ROS_INFO("Shape: %s (N=%d, S=%.2f). Area: %.4f", class_name.c_str(), num_sides, length, area); + } + } + catch(pluginlib::PluginlibException& ex) + { + ROS_ERROR("Failed to load plugin '%s'. Error: %s", class_name.c_str(), ex.what()); + } + catch(const std::exception& e) + { + ROS_ERROR("An unexpected error occurred: %s", e.what()); + } + + return 0; +} \ No newline at end of file diff --git a/pluginlib_11463144/src/polygon_plugins.cpp b/pluginlib_11463144/src/polygon_plugins.cpp new file mode 100644 index 0000000..056bbb3 --- /dev/null +++ b/pluginlib_11463144/src/polygon_plugins.cpp @@ -0,0 +1,17 @@ +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon) +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon) +// ⭐ REMOVED: RegularPolygonN is no longer used +// PLUGINLIB_EXPORT_CLASS(polygon_plugins::RegularPolygonN, polygon_base::RegularPolygon) +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Circle, polygon_base::RegularPolygon) + +// ⭐ ADDED: Export all the new polygon classes +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Pentagon, polygon_base::RegularPolygon) +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Hexagon, polygon_base::RegularPolygon) +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Heptagon, polygon_base::RegularPolygon) +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Octagon, polygon_base::RegularPolygon) +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Nonagon, polygon_base::RegularPolygon) +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Decagon, polygon_base::RegularPolygon) \ No newline at end of file diff --git a/pluginlib_tutorials_/CMakeLists.txt b/pluginlib_tutorials_/CMakeLists.txt new file mode 100644 index 0000000..8ca4684 --- /dev/null +++ b/pluginlib_tutorials_/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 3.0.2) +project(pluginlib_tutorials_) + +## 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 + pluginlib + roscpp +) + +## 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 +# std_msgs # Or other packages containing 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 pluginlib_tutorials_ +# CATKIN_DEPENDS pluginlib roscpp +# 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}/pluginlib_tutorials_.cpp +# ) +add_library(polygon_plugins src/polygon_plugins.cpp) +add_executable(polygon_loader src/polygon_loader.cpp) +target_link_libraries(polygon_loader ${catkin_LIBRARIES}) +## 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/pluginlib_tutorials__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_pluginlib_tutorials_.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/pluginlib_tutorials_/include/pluginlib_tutorials_/polygon_base.h b/pluginlib_tutorials_/include/pluginlib_tutorials_/polygon_base.h new file mode 100644 index 0000000..64914d7 --- /dev/null +++ b/pluginlib_tutorials_/include/pluginlib_tutorials_/polygon_base.h @@ -0,0 +1,15 @@ +#ifndef PLUGINLIB_TUTORIALS__POLYGON_BASE_H_ +#define PLUGINLIB_TUTORIALS__POLYGON_BASE_H_ +namespace polygon_base +{ + class RegularPolygon + { + public: + virtual void initialize(double side_length) = 0; + virtual double area() = 0; + virtual ~RegularPolygon(){} + protected: + RegularPolygon(){} + }; +}; +#endif diff --git a/pluginlib_tutorials_/include/pluginlib_tutorials_/polygon_plugins.h b/pluginlib_tutorials_/include/pluginlib_tutorials_/polygon_plugins.h new file mode 100644 index 0000000..5078316 --- /dev/null +++ b/pluginlib_tutorials_/include/pluginlib_tutorials_/polygon_plugins.h @@ -0,0 +1,53 @@ +#ifndef PLUGINLIB_TUTORIALS__POLYGON_PLUGINS_H_ +#define PLUGINLIB_TUTORIALS__POLYGON_PLUGINS_H_ +#include +#include //cmath標頭檔為特殊運算語法， + //沒有寫的話無法執行pow(,)平方運算語法 + +namespace polygon_plugins +{ + class Triangle : public polygon_base::RegularPolygon + { + public: + Triangle(){} + + void initialize(double side_length) + { + side_length_ = side_length; + } + + double area() + { + return 0.5 * side_length_ * getHeight(); + } + + double getHeight() + { + return sqrt((side_length_ * side_length_) - ((side_length_ / 2) * (side_length_ / 2))); + } + + private: + double side_length_; + }; + + class Square : public polygon_base::RegularPolygon + { + public: + Square(){} + + void initialize(double side_length) + { + side_length_ = side_length; + } + + double area() + { + return side_length_ * side_length_; + } + + private: + double side_length_; + + }; +}; +#endif diff --git a/pluginlib_tutorials_/package.xml b/pluginlib_tutorials_/package.xml new file mode 100644 index 0000000..0afe6e8 --- /dev/null +++ b/pluginlib_tutorials_/package.xml @@ -0,0 +1,64 @@ + + + pluginlib_tutorials_ + 0.0.0 + The pluginlib_tutorials_ package + + + + + joy + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + pluginlib + roscpp + pluginlib + roscpp + pluginlib + roscpp + + + + + + + diff --git a/pluginlib_tutorials_/polygon_plugins.xml b/pluginlib_tutorials_/polygon_plugins.xml new file mode 100644 index 0000000..1578cbd --- /dev/null +++ b/pluginlib_tutorials_/polygon_plugins.xml @@ -0,0 +1,8 @@ + + + This is a triangle plugin. + + + This is a square plugin. + + \ No newline at end of file diff --git a/pluginlib_tutorials_/src/polygon_loader.cpp b/pluginlib_tutorials_/src/polygon_loader.cpp new file mode 100644 index 0000000..7adf02f --- /dev/null +++ b/pluginlib_tutorials_/src/polygon_loader.cpp @@ -0,0 +1,23 @@ +#include +#include + +int main(int argc, char** argv) +{ + pluginlib::ClassLoader poly_loader("pluginlib_tutorials_", "polygon_base::RegularPolygon"); + +try + { + boost::shared_ptr triangle = poly_loader.createInstance("polygon_plugins::Triangle"); + triangle->initialize(10.0); + boost::shared_ptr square = poly_loader.createInstance("polygon_plugins::Square"); + square->initialize(10.0); + ROS_INFO("Triangle area: %.2f", triangle->area()); + ROS_INFO("Square area: %.2f", square->area()); + } + + catch(pluginlib::PluginlibException& ex) + { + ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what()); + } + return 0; +} \ No newline at end of file diff --git a/pluginlib_tutorials_/src/polygon_plugins.cpp b/pluginlib_tutorials_/src/polygon_plugins.cpp new file mode 100644 index 0000000..8b2a263 --- /dev/null +++ b/pluginlib_tutorials_/src/polygon_plugins.cpp @@ -0,0 +1,6 @@ +#include +#include +#include + +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon) +PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon) \ No newline at end of file diff --git a/simple_layers/CMakeLists.txt b/simple_layers/CMakeLists.txt new file mode 100644 index 0000000..98ee7c3 --- /dev/null +++ b/simple_layers/CMakeLists.txt @@ -0,0 +1,209 @@ +cmake_minimum_required(VERSION 3.0.2) +project(simple_layers) + +## 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 + costmap_2d + dynamic_reconfigure + roscpp + rospy + 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 +# 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 simple_layers +# CATKIN_DEPENDS costmap_2d dynamic_reconfigure 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} +) +include_directories(include ${catkin_INCLUDE_DIRS}) +add_library(simple_layer src/simple_layer.cpp) +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/simple_layers.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/simple_layers_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_simple_layers.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/simple_layers/include/simple_layers/simple_layer.h b/simple_layers/include/simple_layers/simple_layer.h new file mode 100644 index 0000000..99fa081 --- /dev/null +++ b/simple_layers/include/simple_layers/simple_layer.h @@ -0,0 +1,28 @@ +#ifndef SIMPLE_LAYER_H_ +#define SIMPLE_LAYER_H_ +#include +#include +#include +#include +#include + +namespace simple_layer_namespace +{ + +class SimpleLayer : public costmap_2d::Layer +{ +public: + SimpleLayer(); + + virtual void onInitialize(); + virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y); + virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + +private: + void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level); + + double mark_x_, mark_y_; + dynamic_reconfigure::Server *dsrv_; +}; +} +#endif diff --git a/simple_layers/ostmap_plugins.xml b/simple_layers/ostmap_plugins.xml new file mode 100644 index 0000000..692bc43 --- /dev/null +++ b/simple_layers/ostmap_plugins.xml @@ -0,0 +1,5 @@ + + + Demo Layer that adds a point 1 meter in front of the robot + + diff --git a/simple_layers/package.xml b/simple_layers/package.xml new file mode 100644 index 0000000..1769c1f --- /dev/null +++ b/simple_layers/package.xml @@ -0,0 +1,73 @@ + + + simple_layers + 0.0.0 + The simple_layers package + + + + + joy + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + costmap_2d + dynamic_reconfigure + roscpp + rospy + std_msgs + costmap_2d + dynamic_reconfigure + roscpp + rospy + std_msgs + costmap_2d + dynamic_reconfigure + roscpp + rospy + std_msgs + + + + + + + diff --git a/simple_layers/src/simple_layer.cpp b/simple_layers/src/simple_layer.cpp new file mode 100644 index 0000000..ecb74bf --- /dev/null +++ b/simple_layers/src/simple_layer.cpp @@ -0,0 +1,54 @@ +#include +#include + +PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::SimpleLayer, costmap_2d::Layer) + +using costmap_2d::LETHAL_OBSTACLE; + +namespace simple_layer_namespace +{ + +SimpleLayer::SimpleLayer() {} + +void SimpleLayer::onInitialize() +{ + ros::NodeHandle nh("~/" + name_); + current_ = true; + + dsrv_ = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = boost::bind( + &SimpleLayer::reconfigureCB, this, _1, _2); + dsrv_->setCallback(cb); +} +void SimpleLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level) +{ + enabled_ = config.enabled; +} + +void SimpleLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, + double* min_y, double* max_x, double* max_y) +{ + if (!enabled_) + return; + + mark_x_ = origin_x + cos(origin_yaw); + mark_y_ = origin_y + sin(origin_yaw); + + *min_x = std::min(*min_x, mark_x_); + *min_y = std::min(*min_y, mark_y_); + *max_x = std::max(*max_x, mark_x_); + *max_y = std::max(*max_y, mark_y_); +} + +void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) + return; + unsigned int mx; + unsigned int my; + if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){ + master_grid.setCost(mx, my, LETHAL_OBSTACLE); + } +} + +} // end namespace diff --git a/tf_demo/CMakeLists.txt b/tf_demo/CMakeLists.txt index 2c944fa..f72c89a 100644 --- a/tf_demo/CMakeLists.txt +++ b/tf_demo/CMakeLists.txt @@ -147,6 +147,11 @@ add_executable(tf_broadcaster src/tf_broadcaster.cpp) # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against +add_executable(tf_listener src/tf_listener.cpp) +# add_executable(tf_mainrobot src/tf_mainrobot.cpp) +add_executable(robot_location src/robot_location.cpp) +target_link_libraries(robot_location ${catkin_LIBRARIES}) +target_link_libraries(tf_listener ${catkin_LIBRARIES}) target_link_libraries(tf_broadcaster ${catkin_LIBRARIES} ) diff --git a/tf_demo/launch/ch5_1.launch b/tf_demo/launch/ch5_1.launch deleted file mode 100644 index 64b9292..0000000 --- a/tf_demo/launch/ch5_1.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/tf_demo/launch/tf_demo.launch b/tf_demo/launch/tf_demo.launch new file mode 100644 index 0000000..325bdae --- /dev/null +++ b/tf_demo/launch/tf_demo.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/tf_demo/launch/tf_demo2.launch b/tf_demo/launch/tf_demo2.launch new file mode 100644 index 0000000..90bf571 --- /dev/null +++ b/tf_demo/launch/tf_demo2.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/tf_demo/src/robot_location.cpp b/tf_demo/src/robot_location.cpp new file mode 100644 index 0000000..6b6457f --- /dev/null +++ b/tf_demo/src/robot_location.cpp @@ -0,0 +1,25 @@ +#include +#include +using namespace std; +int main(int argc, char** argv){ +ros::init(argc, argv, "robot_location"); +ros::NodeHandle node; +tf::TransformListener listener; +ros::Rate rate(2.0); +listener.waitForTransform("/odom", "/base_footprint", ros::Time(0), +ros::Duration(10.0)); +while (ros::ok()){ +tf::StampedTransform transform; +try { +listener.lookupTransform("/odom", "/base_footprint", +ros::Time(0), transform); +double x = transform.getOrigin().x(); +double y = transform.getOrigin().y(); +cout << "Current position: (" << x << "," << y << ")" << endl; +} catch (tf::TransformException &ex) { +ROS_ERROR("%s",ex.what()); +} +rate.sleep(); +} +return 0; +} \ No newline at end of file diff --git a/tf_demo/src/tf_listener.cpp b/tf_demo/src/tf_listener.cpp new file mode 100644 index 0000000..34cd987 --- /dev/null +++ b/tf_demo/src/tf_listener.cpp @@ -0,0 +1,34 @@ +#include +#include +#include +#include +int main(int argc, char** argv) { +ros::init(argc, argv, "my_tf_listener"); +ros::NodeHandle node; +ros::service::waitForService("spawn"); +ros::ServiceClient add_turtle = +node.serviceClient("spawn"); +turtlesim::Spawn srv; +add_turtle.call(srv); +ros::Publisher turtle_vel = +node.advertise("turtle2/cmd_vel", 10); +tf::TransformListener listener; +ros::Rate rate(10.0); +while (node.ok()) { +tf::StampedTransform transform; +try { +listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(10.0)); +listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); +} catch (tf::TransformException ex) { +ROS_ERROR("%s",ex.what()); +} +geometry_msgs::Twist vel_msg; +vel_msg.angular.z = 4 * atan2(transform.getOrigin().y(), +transform.getOrigin().x()); +vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + +pow(transform.getOrigin().y(), 2)); +turtle_vel.publish(vel_msg); +rate.sleep(); +} +return 0; +}; \ No newline at end of file diff --git a/tf_demo/src/tf_mainrobot.cpp b/tf_demo/src/tf_mainrobot.cpp new file mode 100644 index 0000000..10827ac --- /dev/null +++ b/tf_demo/src/tf_mainrobot.cpp @@ -0,0 +1,130 @@ +#include "ros/ros.h" +#include "geometry_msgs/Twist.h" +#include "turtlesim/Pose.h" +#include +#include + +// Define a simple structure for a 2D point (waypoint) +struct Point { + double x; + double y; +}; + +// Global variables to store the turtle's current pose +double pos_x = 0.0; +double pos_y = 0.0; +double theta = 0.0; + +// Waypoints definition: A sequence of (x, y) points for the turtle to follow +// NOTE: Turtlesim's max coordinates are 11.08 x 11.08. +std::vector waypoints = { + {0.0, 5.0}, // Start by turning to this point + {5.0, 0.0}, // Move across + {10.0, 5.0},// Move up + {5.0, 5.0} // Move back +}; + +// Control parameters +const double ANGULAR_SPEED_GAIN = 1.0; // How fast to turn (P-gain for angle) +const double LINEAR_SPEED = 1.0; // Constant forward speed +const double DISTANCE_THRESHOLD = 0.1; // How close to the target is 'close enough' +const double ANGLE_THRESHOLD_RAD = 0.01; // How straight to be before moving forward + +// State variables for waypoint tracking +int current_target_index = 0; +double target_x = waypoints[0].x; +double target_y = waypoints[0].y; + +// --- Callback Function --- +void poseCallback(const turtlesim::PoseConstPtr& msg) +{ + pos_x = msg->x; + pos_y = msg->y; + theta = msg->theta; +} + +// Function to normalize an angle to the range [-pi, pi] +double normalizeAngle(double angle) +{ + while (angle > M_PI) angle -= 2.0 * M_PI; + while (angle < -M_PI) angle += 2.0 * M_PI; + return angle; +} + +// --- Main Program --- +int main(int argc, char **argv) +{ + // Initialize the ROS node + ros::init(argc, argv, "move_turtle_waypoints"); + ros::NodeHandle node; + + // Publisher and Subscriber setup + ros::Publisher pub = node.advertise("turtle1/cmd_vel", 10); + ros::Subscriber sub = node.subscribe("turtle1/pose", 10, poseCallback); + + // Set the loop rate + ros::Rate rate(10); + ROS_INFO("Starting waypoint navigation with %zu points.", waypoints.size()); + + // Main control loop + while (ros::ok()) { + geometry_msgs::Twist msg; // Command message, all fields set to 0 initially + + // 1. Check if all waypoints have been reached + if (current_target_index >= waypoints.size()) { + ROS_INFO("All waypoints reached. Stopping."); + pub.publish(msg); // Publish 0 velocity to stop + break; // Exit the loop + } + + // 2. Calculate the distance to the current target + double dx = target_x - pos_x; + double dy = target_y - pos_y; + double distance = std::sqrt(dx * dx + dy * dy); + + // 3. Check if the target is reached + if (distance < DISTANCE_THRESHOLD) { + ROS_INFO("Waypoint %d reached: (%.2f, %.2f)", + current_target_index, target_x, target_y); + + // Advance to the next waypoint + current_target_index++; + if (current_target_index < waypoints.size()) { + target_x = waypoints[current_target_index].x; + target_y = waypoints[current_target_index].y; + } + // Stop and continue the loop to re-evaluate the distance for the new target + msg.linear.x = 0.0; + msg.angular.z = 0.0; + } else { + // 4. Calculate the desired angle to the target + double target_angle = std::atan2(dy, dx); + + // 5. Calculate the angle error + double angle_error = normalizeAngle(target_angle - theta); + + // 6. Set angular velocity (Turn towards the target) + msg.angular.z = ANGULAR_SPEED_GAIN * angle_error; + + // 7. Set linear velocity (Move forward only if we are facing the target) + // Use the absolute value of the error for comparison + if (std::abs(angle_error) < ANGLE_THRESHOLD_RAD) { + msg.linear.x = LINEAR_SPEED; + msg.angular.z = 0.0; // Stop turning while moving straight + } else { + msg.linear.x = 0.0; // Stop moving while turning + } + } + + // Publish the movement commands + pub.publish(msg); + + // Allow ROS to process events (like receiving the pose) + ros::spinOnce(); + + // Wait until the next loop cycle + rate.sleep(); + } + + return 0; +} \ No newline at end of file