compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Add drive arc functionality

Browse source
This commit is contained in:
Matthias Guzmits 2023-07-19 18:36:07 +02:00
parent de112d98e4
commit ed04957d94
3 changed files with 89 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

13
compLib/include/motor.h
View file

@ -10,6 +10,7 @@
#include "rclcpp_action/rclcpp_action.hpp"
#include "irobot_create_msgs/action/drive_distance.hpp"
#include "irobot_create_msgs/action/drive_arc.hpp"
#include "irobot_create_msgs/action/rotate_angle.hpp"
class DriveDistNode : public rclcpp::Node
@ -51,4 +52,16 @@ private:
bool processing;
};
class DriveArcNode : public rclcpp::Node
{
public:
DriveArcNode();
void drive_arc(float angle, float radius, float velocity, int direction=1);
void kill();
private:
void result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveArc>::WrappedResult & result);
rclcpp_action::Client<irobot_create_msgs::action::DriveArc>::SharedPtr drive_arc_action_;
bool processing;
};
#endif

14
compLib/src/main.cpp
View file

@ -23,6 +23,11 @@ void run_node2(std::shared_ptr<RotateAngleNode> node)
rclcpp::spin(node);
}
void run_node3(std::shared_ptr<DriveArcNode> node)
{
rclcpp::spin(node);
}
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
@ -30,16 +35,19 @@ int main(int argc, char * argv[])
auto ddn = std::make_shared<DriveDistNode>();
auto ssn = std::make_shared<SetSpeedNode>();
auto ran = std::make_shared<RotateAngleNode>();
auto dan = std::make_shared<DriveArcNode>();
std::thread t;
std::thread t1;
std::thread t2;
std::thread t3;
t = std::thread(run_node, ddn);
t1 = std::thread(run_node1, ssn);
t2 = std::thread(run_node2, ran);
t3 = std::thread(run_node3, dan);
ssn->drive(0.2);
ssn->drive(0.3);
std::this_thread::sleep_for (std::chrono::milliseconds(2000));
@ -54,13 +62,17 @@ int main(int argc, char * argv[])
// std::this_thread::sleep_for (std::chrono::milliseconds(5000));
dan->drive_arc(90, 0.5, 0.5);
ddn->kill();
ssn->kill();
ddn->kill();
dan->kill();
t.join();
t1.join();
t2.join();
t3.join();
return 0;
}

72
compLib/src/motor.cpp
View file

@ -10,6 +10,7 @@
#include "rclcpp_action/rclcpp_action.hpp"
#include "irobot_create_msgs/action/drive_distance.hpp"
#include "irobot_create_msgs/action/drive_arc.hpp"
#include "irobot_create_msgs/action/rotate_angle.hpp"
#include "motor.h"
@ -28,7 +29,7 @@ DriveDistNode::DriveDistNode()
void DriveDistNode::drive_dist(float meters, float velocity)
{
std::cout << "drive dist" << std::endl;
RCLCPP_INFO(this->get_logger(), "drive dist");
processing = true;
auto data = irobot_create_msgs::action::DriveDistance::Goal();
auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::DriveDistance>::SendGoalOptions();
@ -45,9 +46,9 @@ void DriveDistNode::drive_dist(float meters, float velocity)
void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveDistance>::WrappedResult & result)
{
processing = false;
std::cout << "finished dist" << std::endl;
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "finished dist");
return;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
@ -63,7 +64,7 @@ void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot
void DriveDistNode::kill()
{
std::cout << "DriveDistNode killed" << std::endl;
RCLCPP_INFO(this->get_logger(), "DriveDistNode killed");
rclcpp::shutdown();
}
@ -78,7 +79,6 @@ SetSpeedNode::SetSpeedNode()
void SetSpeedNode::drive_loop(float speed)
{
std::cout << "Loop" << std::endl;
while (run)
{
set_speed(speed);
@ -102,20 +102,20 @@ void SetSpeedNode::set_speed(float speed)
void SetSpeedNode::drive(float speed)
{
std::cout << "Drive" << std::endl;
RCLCPP_INFO(this->get_logger(), "Start drive");
run = true;
t = std::thread(&SetSpeedNode::drive_loop, this, speed);
}
void SetSpeedNode::stop()
{
std::cout << "stop" << std::endl;
run = false;
RCLCPP_INFO(this->get_logger(), "Stop drive");
}
void SetSpeedNode::kill()
{
std::cout << "SetSpeedNode killed" << std::endl;
RCLCPP_INFO(this->get_logger(), "SetSpeedNode killed");
rclcpp::shutdown();
}
@ -148,9 +148,9 @@ void RotateAngleNode::rotate_angle(float angle, float velocity)
void RotateAngleNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::RotateAngle>::WrappedResult & result)
{
processing = false;
std::cout << "finished rotation" << std::endl;
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "finished rotation");
return;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
@ -166,6 +166,60 @@ void RotateAngleNode::result_callback(const rclcpp_action::ClientGoalHandle<irob
void RotateAngleNode::kill()
{
std::cout << "RotateAngleNode killed" << std::endl;
RCLCPP_INFO(this->get_logger(), "RotateAngleNode killed");
rclcpp::shutdown();
}
DriveArcNode::DriveArcNode()
: Node("drive_arc_node")
{
drive_arc_action_ = rclcpp_action::create_client<irobot_create_msgs::action::DriveArc>(
this,
"/drive_arc"
);
}
void DriveArcNode::drive_arc(float angle, float radius, float velocity, int direction)
{
processing = true;
angle *= pi / 180;
auto data = irobot_create_msgs::action::DriveArc::Goal();
auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::DriveArc>::SendGoalOptions();
send_goal_options.result_callback =
std::bind(&DriveArcNode::result_callback, this, std::placeholders::_1);
data.angle = angle;
data.radius = radius;
data.translate_direction = direction;
data.max_translation_speed = velocity;
drive_arc_action_->async_send_goal(data, send_goal_options);
while (processing) {}
}
void DriveArcNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveArc>::WrappedResult & result)
{
processing = false;
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_logger(), "finished arc");
return;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
}
void DriveArcNode::kill()
{
RCLCPP_INFO(this->get_logger(), "DriveArcNode killed");
rclcpp::shutdown();
}