px3g9aoef
/
ros2_navigation_assignment
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: px3g9aoef:main
Branches Tags
px3g9aoef:master
px3g9aoef:main
..
compare: px3g9aoef:master
Branches Tags
px3g9aoef:master
px3g9aoef:main

No commits in common. 'main' and 'master' have entirely different histories.
main ... master

6 changed files with 273 additions and 36 deletions
Show Stats

34
.gitignore vendored
Unescape View File

@ -1,34 +0,0 @@
# ---> C++
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app

2
README.md
Unescape View File

@ -1,2 +0,0 @@
# ros2_navigation_assignment

29
src/navigation_assignment/test/CMakeLists.txt
Unescape View File

@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 3.8)
project(navigation_assignment)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
add_executable(slam_navigation src/slam_navigation.cpp)
ament_target_dependencies(slam_navigation rclcpp geometry_msgs)
add_executable(t_shape_movement src/t_shape_movement.cpp)
ament_target_dependencies(t_shape_movement rclcpp geometry_msgs)
install(TARGETS
slam_navigation
t_shape_movement
DESTINATION lib/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

21
src/navigation_assignment/test/package.xml
Unescape View File

@ -0,0 +1,21 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>navigation_assignment</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="chenlei@todo.todo">chenlei</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

70
src/navigation_assignment/test/src/slam_navigation.cpp
Unescape View File

@ -0,0 +1,70 @@
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "chrono"
#include "memory"
using namespace std::chrono_literals;
class SLAMNavigation : public rclcpp::Node
{
public:
SLAMNavigation() : Node("slam_navigation")
{
publisher_ = this ->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel",10);
timer_t =this->create_wall_timer(100ms, std::bind(&SLAMNavigation::navigate, this));
start_time_ =this->now();
RCLCPP_INFO(this->get_logger(),"SLAM Navigation START");
}
private:
void navigate()
{
auto current_time = this ->now();
auto elapsed_time = (current_time - start_time_).seconds();
auto twist_msg = std::make_unique<geometry_msgs::msg::Twist>();
if (elapsed_time < 5.0) {
twist_msg->linear.x = 0.2;
twist_msg->angular.z = 0.0;
RCLCPP_INFO(this->get_logger(),"GO STRAIGHT");
}
else if (elapsed_time < 8.0) {
twist_msg->linear.x = 0.1;
twist_msg->angular.z = -0.5;
RCLCPP_INFO(this->get_logger(),"TURN RIGHT");
}
else if (elapsed_time < 13.0) {
twist_msg->linear.x = 0.2;
twist_msg->angular.z = 0.0;
RCLCPP_INFO(this->get_logger(),"GO STRAIGHT");
}
else if (elapsed_time < 16.0) {
twist_msg->linear.x = 0.1;
twist_msg->angular.z = -0.5;
RCLCPP_INFO(this->get_logger(),"TURN RIGHT");
}
else if (elapsed_time < 16.0) {
twist_msg->linear.x = 0.1;
twist_msg->angular.z = -0.5;
RCLCPP_INFO(this->get_logger(),"GO BACK");
}
else {
twist_msg->linear.x = 0.0;
twist_msg->angular.z = 0.0;
RCLCPP_INFO(this _>get_logger(),"OVER")
}
publisher_->publisher(std::move(twist_nsg));
}
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time start_time_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
auto node = std:;make_shared<SLAMNavigation>();
rclcpp::spin(node);
rclcpp::shutdoen();
return 0;
}

153
src/navigation_assignment/test/src/t_shape_movement.cpp
Unescape View File

@ -0,0 +1,153 @@
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "chrono"
#include "memory"
using namespace std::chrono_literals;
class TShapeMovement : public rclcpp::Node
{
public:
TShapeMovement() : Node("t_shape_navigation")
{
publisher_ = this ->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel",10);
timer_=this->create_wall_timer(100ms, std::bind(&SLAMNavigation::navigate, this));
phase_time =this->now();
cycle_count_ = 0;
max_cycles_ = 3;
RCLCPP_INFO(this->get_logger(),"SLAM Navigation START");
}
private:
void navigate()
{
auto currnet_time = this->now();
auto slapsed_time = (current_time - phase_start_time_).seconds();
auto twist_msg = std::make_nuique<geometry_msgs::msg::Twist>();
bool phase_completed = false;
switch(current_phase_) {
case 0:
if (elapsed_time < 3.0) {
twist_msg->linear.x = 0.2;
twist_msg->angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "GO STRAIGHT");
}
else {
phase_complete = true;
}
break;
case 1:
if (elapsed_time < 1.0) {
twist_msg->linear.x = 0.0;
twist_msg->angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "ADJUST");
}
else {
phase_complete = true;
}
break;
case 2:
if (elapsed_time < 2.0) {
twist_msg->linear.x = 0.0;
twist_msg->angular.z = 0.5;
RCLCPP_INFO(this->get_logger(), "TURN RIGHT");
}
else {
phase_complete = true;
}
break;
case 3:
if (elapsed_time < 2.0) {
twist_msg->linear.x = 0.2;
twist_msg->angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "GO STRAIGHT");
}
else {
phase_complete = true;
}
break;
case 4:
if (elapsed_time < 3.0) {
twist_msg->linear.x = -0.2;
twist_msg->angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "GO BACK");
}
else {
phase_complete = true;
}
break;
case 5:
if (elapsed_time < 2.0) {
twist_msg->linear.x = 0.0;
twist_msg->angular.z = -0.5;
RCLCPP_INFO(this->get_logger(), "TRUN RIGHT");
}
else {
phase_complete = true;
}
break;
case 6:
if (elapsed_time < 3.0) {
twist_msg->linear.x = 0.2;
twist_msg->angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "GO STRAIGHT");
}
else {
phase_complete = true;
}
break;
case 7:
if (elapsed_time < 3.0) {
twist_msg->linear.x = -0.2;
twist_msg->angular.z = 0.0;
RCLCPP_INFO(this->get_logger(), "GO BACK");
}
else {
phase_complete = true;
}
break;
}
publisher_->publish(std::move(twist_msg));
if (phase_completed) {
current_phase_++;
phase_start_time_ =this->now();
if (current_phase_ >7) {
cycle_count_++;
current_phase_ =0;
if(cycle_count_ >= max_cycles_) {
RCLCPP_INFO(this->get_logger(),"number %d",max_cycles-);
timer_->cancel();
}else {
RCLCPP_INFO(this->get_logger(),"begin %d",cycle_count_+1);
}
}
}
}
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Time phase_start_time_;
int current_phase_ = 0;
int cycle_count_;
int max_cycles_;
};
int main(int argc,char * argv[])
{
rclcpp::init(argc,argv);
auto node = std::make_shared<TShapeMovement>();
rclcpp::spin(node);
rclcpp::shutdown();
retrun 0;
}
Write Preview
Loading…
Cancel
Save