diff --git a/catkin_ws/src/asclinic_pkg/CMakeLists.txt b/catkin_ws/src/asclinic_pkg/CMakeLists.txt index e682b6fb1796b0663ad67081fb8bb5224d046405..c6bf59f219126dff6c3c88adf9eade5152975adb 100644 --- a/catkin_ws/src/asclinic_pkg/CMakeLists.txt +++ b/catkin_ws/src/asclinic_pkg/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS rospy std_msgs geometry_msgs + tf2_ros tf sensor_msgs cv_bridge @@ -99,6 +100,7 @@ generate_messages( std_msgs geometry_msgs sensor_msgs + ) ################################################ @@ -221,6 +223,7 @@ add_executable(fcontroller1 src/ascr/fcontroller1.cpp) add_executable(final_controller src/ascr/final_controller.cpp) add_executable(control_node src/forklift/control_node.cpp) add_executable(odometry_new src/forklift/odometry_new.cpp) +add_executable(tftransfer src/forklift/tftransfer.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -257,6 +260,7 @@ add_dependencies(fcontroller1 asclinic_pkg_generate_mes add_dependencies(final_controller asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) add_dependencies(control_node asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) add_dependencies(odometry_new asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) +add_dependencies(tftransfer asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) @@ -290,6 +294,7 @@ target_link_libraries(fcontroller1 ${catkin_LIBRARIES}) target_link_libraries(final_controller ${catkin_LIBRARIES}) target_link_libraries(control_node ${catkin_LIBRARIES}) target_link_libraries(odometry_new ${catkin_LIBRARIES}) +target_link_libraries(tftransfer ${catkin_LIBRARIES}) diff --git a/catkin_ws/src/asclinic_pkg/launch/forklift2.launch b/catkin_ws/src/asclinic_pkg/launch/forklift2.launch new file mode 100644 index 0000000000000000000000000000000000000000..489693bf32315445cdca3d1fbd75a04b893dc74a --- /dev/null +++ b/catkin_ws/src/asclinic_pkg/launch/forklift2.launch @@ -0,0 +1,38 @@ +<launch> + <!-- Launch the tf transfer node --> + <node + pkg="asclinic_pkg" + type="tftransfer" + name="tftransfer" + /> + + <!-- Launch the I2C for Motors node --> + <node + pkg="asclinic_pkg" + type="odometry_new" + name="odometry_new" + /> + + <!-- Launch the control node --> + <node + pkg="asclinic_pkg" + type="control_node" + name="control_node" + /> + + <!-- Launch the RPLIDAR node within the "asc" namespace --> + <group ns="asc"> + <node + pkg="rplidar_ros" + type="rplidarNode" + name="rplidarNode" + output="screen" + > + <param name="serial_port" type="string" value="/dev/rplidar"/> + <param name="serial_baudrate" type="int" value="115200"/> + <param name="frame_id" type="string" value="laser"/> + <param name="inverted" type="bool" value="false"/> + <param name="angle_compensate" type="bool" value="true"/> + </node> + </group> +</launch> diff --git a/catkin_ws/src/asclinic_pkg/launch/gmapping.launch b/catkin_ws/src/asclinic_pkg/launch/gmapping.launch index a10269f6c88e89d5d940bc7cf0c812fef03056a2..574d231b6bf650afc9edbb2c6b262a8478950198 100644 --- a/catkin_ws/src/asclinic_pkg/launch/gmapping.launch +++ b/catkin_ws/src/asclinic_pkg/launch/gmapping.launch @@ -1,18 +1,6 @@ <launch> <!-- Launch the RPLIDAR node --> - <group ns="asc"> - <node - pkg="rplidar_ros" - name="rplidarNode" - output="screen" - type="rplidarNode"> - <param name="serial_port" type="string" value="/dev/rplidar"/> - <param name="serial_baudrate" type="int" value="115200"/> - <param name="frame_id" type="string" value="laser"/> - <param name="inverted" type="bool" value="false"/> - <param name="angle_compensate" type="bool" value="true"/> - </node> - </group> + <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_laser" args="0 0 1 0 0 0 base_link laser" /> diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/tftransfer.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/tftransfer.cpp new file mode 100644 index 0000000000000000000000000000000000000000..28a2805452a3d3b15b2a14bcfb7af2e1add40a40 --- /dev/null +++ b/catkin_ws/src/asclinic_pkg/src/forklift/tftransfer.cpp @@ -0,0 +1,37 @@ +#include <ros/ros.h> +#include <tf2_ros/transform_broadcaster.h> +#include <geometry_msgs/TransformStamped.h> +#include <nav_msgs/Odometry.h> + +// 里程计回调函数,用于发布 TF 变换 +void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) +{ + static tf2_ros::TransformBroadcaster br; + geometry_msgs::TransformStamped transformStamped; + + // 填充变换消息 + transformStamped.header.stamp = msg->header.stamp; + transformStamped.header.frame_id = "odom"; + transformStamped.child_frame_id = "base_link"; + + transformStamped.transform.translation.x = msg->pose.pose.position.x; + transformStamped.transform.translation.y = msg->pose.pose.position.y; + transformStamped.transform.translation.z = msg->pose.pose.position.z; + transformStamped.transform.rotation = msg->pose.pose.orientation; + + // 发布变换 + br.sendTransform(transformStamped); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "tftransfer"); + ros::NodeHandle nh; + + // 订阅 /odom 话题 + ros::Subscriber sub = nh.subscribe("/odom", 1, odomCallback); + + ros::spin(); + + return 0; +}