joy/ROS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Nov 13 checkpoint 1

Browse Source
This commit is contained in:
joy 2025-11-13 15:15:15 +08:00
parent f41536aef0
commit 27bfcff45c
35 changed files with 2169 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ch5_pkg/launch/ch5_3.launch
View File

@ -3,7 +3,7 @@
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="multi_robot_name" default=""/>
<!-- loading the world --> 
<!-- loading the world -->
<include file="$(find turtlebot3_gazebo)/launch/turtlebot3_house_sky.launch"/>
<!-- Run the map server -->

207
ch7_pkg/CMakeLists.txt Normal file
View File

@ -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)

43
ch7_pkg/launch/ch7_1.launch Normal file
View File

@ -0,0 +1,43 @@
<launch>
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="multi_robot_name" default=""/>
<param name="/use_sim_time" value="true"/>
<arg name="move_forward_only" default="false"/>
<arg name="open_rviz" default="true"/>
<!-- TurtleBot3 -->
<include file="/home/joy/catkin/src/turtlebot3/turtlebot3_bringup/launch/turtlebot3_remote.launch">
<arg name="model" value="$(arg model)" />
</include>
<!-- Run Gazebo with turtlebot -->
<include file="/home/joy/catkin/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_world.launch"/>
<!-- Load existing map -->
<node name="map_server" pkg="map_server" type="map_server" args="/home/joy/catkin/src/ch7_pkg/mymap.yaml" />
<!-- Publish a static transformation between /odom and /map -->
<node name="tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /map/odom 100" />
<!-- move_base -->
<include file="$(find turtlebot3_navigation)/launch/move_base.launch">
<arg name="model" value="$(arg model)" />
<arg name="move_forward_only" value="$(arg move_forward_only)"/>
</include>
<!-- AMCL -->
<include file="$(find turtlebot3_navigation)/launch/amcl.launch"/>
<!-- rviz -->
<group if="$(arg open_rviz)">
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
</group>
</launch>

5
ch7_pkg/map.pgm Normal file
View File

File diff suppressed because one or more lines are too long

7
ch7_pkg/map.yaml Normal file
View File

@ -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

5
ch7_pkg/mymap.pgm Normal file
View File

File diff suppressed because one or more lines are too long

7
ch7_pkg/mymap.yaml Normal file
View File

@ -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

71
ch7_pkg/package.xml Normal file
View File

@ -0,0 +1,71 @@
<?xml version="1.0"?>
<package format="2">
<name>ch7_pkg</name>
<version>0.0.0</version>
<description>The ch7_pkg package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="joy@todo.todo">joy</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ch7_pkg</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>tf</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

283
my_turtle/src/MoveTurtle.cpp
View File

@ -1,119 +1,202 @@
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
#include <cmath>
#include <vector>
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<Point> 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<geometry_msgs::Twist>("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;
}

214
pluginlib_11463144/CMakeLists.txt Normal file
View File

@ -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)

15
pluginlib_11463144/include/pluginlib_11463144/polygon_base.h Normal file
View File

@ -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

198
pluginlib_11463144/include/pluginlib_11463144/polygon_plugins.h Normal file
View File

@ -0,0 +1,198 @@
#ifndef PLUGINLIB_TUTORIALS__POLYGON_PLUGINS_H_
#define PLUGINLIB_TUTORIALS__POLYGON_PLUGINS_H_
#include <pluginlib_11463144/polygon_base.h> // 假設你的路徑是這個，ROS 教學中通常是 <pluginlib_tutorials_/polygon_base.h>
#include <cmath> //cmath標頭檔為特殊運算語法，
//沒有寫的話無法執行pow(,)平方運算語法
#include <stdexcept> // 用於拋出例外
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_

64
pluginlib_11463144/package.xml Normal file
View File

@ -0,0 +1,64 @@
<?xml version="1.0"?>
<package format="2">
<name>pluginlib_11463144</name>
<version>0.0.0</version>
<description>The pluginlib_11463144 package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="joy@todo.todo">joy</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/pluginlib_11463144</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>pluginlib</build_depend>
<build_depend>roscpp</build_depend>
<build_export_depend>pluginlib</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>pluginlib</exec_depend>
<exec_depend>roscpp</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<pluginlib_11463144 plugin="${prefix}/polygon_plugins.xml" />
</export>
</package>

33
pluginlib_11463144/polygon_plugins.xml Normal file
View File

@ -0,0 +1,33 @@
<library path="lib/libpolygon_plugins">
<class name="polygon_plugins::Triangle" type="polygon_plugins::Triangle" base_class_type="polygon_base::RegularPolygon">
<description>A regular triangle plugin.</description>
</class>
<class name="polygon_plugins::Square" type="polygon_plugins::Square" base_class_type="polygon_base::RegularPolygon">
<description>A regular square plugin.</description>
</class>
<class name="polygon_plugins::Circle" type="polygon_plugins::Circle" base_class_type="polygon_base::RegularPolygon">
<description>Circle approximation plugin.</description>
</class>
<class name="polygon_plugins::Pentagon" type="polygon_plugins::Pentagon" base_class_type="polygon_base::RegularPolygon">
<description>A regular pentagon plugin.</description>
</class>
<class name="polygon_plugins::Hexagon" type="polygon_plugins::Hexagon" base_class_type="polygon_base::RegularPolygon">
<description>A regular hexagon plugin.</description>
</class>
<class name="polygon_plugins::Heptagon" type="polygon_plugins::Heptagon" base_class_type="polygon_base::RegularPolygon">
<description>A regular heptagon plugin.</description>
</class>
<class name="polygon_plugins::Octagon" type="polygon_plugins::Octagon" base_class_type="polygon_base::RegularPolygon">
<description>A regular octagon plugin.</description>
</class>
<class name="polygon_plugins::Nonagon" type="polygon_plugins::Nonagon" base_class_type="polygon_base::RegularPolygon">
<description>A regular nonagon plugin.</description>
</class>
<class name="polygon_plugins::Decagon" type="polygon_plugins::Decagon" base_class_type="polygon_base::RegularPolygon">
<description>A regular decagon plugin.</description>
</class>
</library>

111
pluginlib_11463144/src/polygon_loader.cpp Normal file
View File

@ -0,0 +1,111 @@
#include <pluginlib/class_loader.h>
#include <pluginlib_11463144/polygon_base.h> // 替換為你的實際路徑
#include <ros/ros.h> // 引入 ROS/ROS_INFO
#include <stdexcept>
#include <string>
#include <cstdlib> // 用於 std::atoi 和 std::stod
int main(int argc, char** argv)
{
// 1. 檢查參數數量
if (argc != 3)
{
ROS_ERROR("Usage: polygon_plugin_tester <number_of_sides> <side_length_or_radius>");
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<polygon_base::RegularPolygon> poly_loader(
"pluginlib_11463144",
"polygon_base::RegularPolygon"
);
boost::shared_ptr<polygon_base::RegularPolygon> 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;
}

17
pluginlib_11463144/src/polygon_plugins.cpp Normal file
View File

@ -0,0 +1,17 @@
#include <pluginlib/class_list_macros.h>
#include <pluginlib_11463144/polygon_base.h>
#include <pluginlib_11463144/polygon_plugins.h>
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)

205
pluginlib_tutorials_/CMakeLists.txt Normal file
View File

@ -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)

15
pluginlib_tutorials_/include/pluginlib_tutorials_/polygon_base.h Normal file
View File

@ -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

53
pluginlib_tutorials_/include/pluginlib_tutorials_/polygon_plugins.h Normal file
View File

@ -0,0 +1,53 @@
#ifndef PLUGINLIB_TUTORIALS__POLYGON_PLUGINS_H_
#define PLUGINLIB_TUTORIALS__POLYGON_PLUGINS_H_
#include <pluginlib_tutorials_/polygon_base.h>
#include <cmath> //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

64
pluginlib_tutorials_/package.xml Normal file
View File

@ -0,0 +1,64 @@
<?xml version="1.0"?>
<package format="2">
<name>pluginlib_tutorials_</name>
<version>0.0.0</version>
<description>The pluginlib_tutorials_ package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="joy@todo.todo">joy</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/pluginlib_tutorials_</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>pluginlib</build_depend>
<build_depend>roscpp</build_depend>
<build_export_depend>pluginlib</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>pluginlib</exec_depend>
<exec_depend>roscpp</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<pluginlib_tutorials_ plugin="${prefix}/polygon_plugins.xml" />
</export>
</package>

8
pluginlib_tutorials_/polygon_plugins.xml Normal file
View File

@ -0,0 +1,8 @@
<library path="lib/libpolygon_plugins">
<class type="polygon_plugins::Triangle" base_class_type="polygon_base::RegularPolygon">
<description>This is a triangle plugin.</description>
</class>
<class type="polygon_plugins::Square" base_class_type="polygon_base::RegularPolygon">
<description>This is a square plugin.</description>
</class>
</library>

23
pluginlib_tutorials_/src/polygon_loader.cpp Normal file
View File

@ -0,0 +1,23 @@
#include <pluginlib/class_loader.h>
#include <pluginlib_tutorials/polygon_base.h>
int main(int argc, char** argv)
{
pluginlib::ClassLoader<polygon_base::RegularPolygon> poly_loader("pluginlib_tutorials_", "polygon_base::RegularPolygon");
try
{
boost::shared_ptr<polygon_base::RegularPolygon> triangle = poly_loader.createInstance("polygon_plugins::Triangle");
triangle->initialize(10.0);
boost::shared_ptr<polygon_base::RegularPolygon> 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;
}

6
pluginlib_tutorials_/src/polygon_plugins.cpp Normal file
View File

@ -0,0 +1,6 @@
#include <pluginlib/class_list_macros.h>
#include <pluginlib_tutorials_/polygon_base.h>
#include <pluginlib_tutorials_/polygon_plugins.h>
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Triangle, polygon_base::RegularPolygon)
PLUGINLIB_EXPORT_CLASS(polygon_plugins::Square, polygon_base::RegularPolygon)

209
simple_layers/CMakeLists.txt Normal file
View File

@ -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)

28
simple_layers/include/simple_layers/simple_layer.h Normal file
View File

@ -0,0 +1,28 @@
#ifndef SIMPLE_LAYER_H_
#define SIMPLE_LAYER_H_
#include <ros/ros.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/GenericPluginConfig.h>
#include <dynamic_reconfigure/server.h>
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<costmap_2d::GenericPluginConfig> *dsrv_;
};
}
#endif

5
simple_layers/ostmap_plugins.xml Normal file
View File

@ -0,0 +1,5 @@
<library path="lib/libsimple_layer">
<class type="simple_layer_namespace::SimpleLayer" base_class_type="costmap_2d::Layer">
<description>Demo Layer that adds a point 1 meter in front of the robot</description>
</class>
</library>

73
simple_layers/package.xml Normal file
View File

@ -0,0 +1,73 @@
<?xml version="1.0"?>
<package format="2">
<name>simple_layers</name>
<version>0.0.0</version>
<description>The simple_layers package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="joy@todo.todo">joy</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/simple_layers</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>costmap_2d</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>costmap_2d</build_export_depend>
<build_export_depend>dynamic_reconfigure</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>costmap_2d</exec_depend>
<exec_depend>dynamic_reconfigure</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<costmap_2d plugin="${prefix}/costmap_plugins.xml" />
</export>
</package>

54
simple_layers/src/simple_layer.cpp Normal file
View File

@ -0,0 +1,54 @@
#include<simple_layers/simple_layer.h>
#include <pluginlib/class_list_macros.h>
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<costmap_2d::GenericPluginConfig>(nh);
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::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

5
tf_demo/CMakeLists.txt
View File

@ -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}
)

3
tf_demo/launch/ch5_1.launch
View File

@ -1,3 +0,0 @@
<launch>
<include file="$(find turtlebot3_gazebo)/launch/turtlebot3_house_sky.launch"/>
</launch>

31
tf_demo/launch/tf_demo.launch Normal file
View File

@ -0,0 +1,31 @@
<launch>
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="multi_robot_name" default=""/>
<param name="/use_sim_time" value="true"/>
<!-- loading the world -->
<include file="$(find turtlebot3_gazebo)/launch/turtlebot3_house_sky.launch"/>
<!-- Run the map server -->
<arg name="map_file" default="$(find ch5_pkg)/mymap.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- TurtleBot3 -->
<include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
<arg name="model" value="$(arg model)" />
</include>
<!-- rviz -->
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find turtlebot3_description)/rviz/model.rviz"/>
<node name="tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /map /odom 100"/>
<!-- Publish a static transformation between /odom and /map
<node name="tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /map /odom 100" />
-->
<node name="robot_location" pkg="tf_demo" type="robot_location" output="screen"/>
</launch>

20
tf_demo/launch/tf_demo2.launch Normal file
View File

@ -0,0 +1,20 @@
<launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- <node pkg="my_turtle" type="moveTurtle" name="MoveTurtle" output="screen"/> -->
<node name="move_turtle" pkg="my_turtle" type="MoveTurtle" output="screen" />
<!-- tf broadcaster node -->
<node pkg="tf_demo" type="tf_broadcaster"
args="/turtle1" name="turtle1_tf_broadcaster" />
<!-- Second broadcaster node -->
<node pkg="tf_demo" type="tf_broadcaster"
args="/turtle2" name="turtle2_tf_broadcaster" />
<!-- tf listener node -->
<!-- <node pkg="tf_demo" type="MoveTurtle" name="MoveTurtle" output="screen" /> -->
<!-- tf listener node -->
<node pkg="tf_demo" type="tf_listener" name="tf_listener" output="screen" />
</launch>

25
tf_demo/src/robot_location.cpp Normal file
View File

@ -0,0 +1,25 @@
#include <ros/ros.h>
#include <tf/transform_listener.h>
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;
}

34
tf_demo/src/tf_listener.cpp Normal file
View File

@ -0,0 +1,34 @@
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
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<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("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;
};

130
tf_demo/src/tf_mainrobot.cpp Normal file
View File

@ -0,0 +1,130 @@
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
#include <cmath>
#include <vector>
// 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<Point> 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<geometry_msgs::Twist>("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;
}