Skip to content

ibus control and Manual and automatic switching #11

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
robox-xx wants to merge 1 commit into
base: master
Choose a base branch
from
Open

ibus control and Manual and automatic switching #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ add_executable(test_server test/treeNode/test_server.cpp)
add_dependencies(test_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_server ${catkin_LIBRARIES} )

add_executable(${PROJECT_NAME} src/move_base_client.cpp src/control_loop.cpp src/laser_goal.cpp src/ChassisMove.cpp)
add_executable(${PROJECT_NAME} src/move_base_client.cpp src/control_loop.cpp src/Move_to_targe.cpp src/Chassis_Control.cpp src/ChassisMove.cpp)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} )

Expand Down
43 changes: 43 additions & 0 deletions include/rc_decision/aurora/Chassis_Control.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
/*
* @Author: robox-xx [email protected]
* @Date: 2023-03-15 11:04:27
* @LastEditors: robox-xx [email protected]
* @LastEditTime: 2023-04-17 14:19:33
*/
#pragma once
#include <ros/ros.h>
#include <iostream>
#include <move_base_msgs/MoveBaseAction.h>
#include <behaviortree_cpp_v3/action_node.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Twist.h>
#include <rc_msgs/IbusData.h>
#include <std_msgs/Float64MultiArray.h>


namespace rc_decision {

class Chassis_Control : public BT::SyncActionNode {
public:
Chassis_Control (const std::string& name, const BT::NodeConfiguration& config) : BT::SyncActionNode(name, config)
{
pub_cmdvel = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
};

// 这个必须要写，处理接口参数的
static BT::PortsList providedPorts() {
BT::PortsList ports_list;
//拿出黑板数据的初始化
ports_list.insert(BT::InputPort<rc_msgs::IbusData>("Ibusdata"));
return ports_list;
}

BT::NodeStatus tick() override;

private:
ros::NodeHandle nh;
ros::Publisher pub_cmdvel;
geometry_msgs::Twist vel;
};

}
35 changes: 16 additions & 19 deletions include/rc_decision/aurora/laser_goal.h → include/rc_decision/aurora/Move_to_targe.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
/*
* @Author: robox-xx [email protected]
* @Date: 2023-03-29 14:23:28
* @LastEditors: robox-xx 1399786770@qq.com
* @LastEditTime: 2023-04-03 14:21:56
* @LastEditors: robox-xx [email protected].com
* @LastEditTime: 2023-04-18 15:46:13
* @FilePath: /rc_ws/src/rc_decision/include/rc_decision/aurora/laser_goal.h
*/

Expand All @@ -11,6 +11,7 @@
#include<chrono>
#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <behaviortree_cpp_v3/action_node.h>
#include "behaviortree_cpp_v3/behavior_tree.h"
#include <move_base_msgs/MoveBaseAction.h>
Expand All @@ -20,14 +21,14 @@

namespace rc_decision {

class laser_goal : public BT::StatefulActionNode {
class Move_to_targe : public BT::AsyncActionNode {
public:
laser_goal(const std::string& name, const BT::NodeConfiguration& config)
: BT::StatefulActionNode(name, config),
ac("move_base", true)
Move_to_targe(const std::string& name, const BT::NodeConfiguration& config)
: BT::AsyncActionNode(name, config),
ac_("move_base", true)
{
ros::NodeHandle n;
radar_org_data = n.subscribe("/radar/shootPosition", 5, &laser_goal::LaserCallback, this);
pub_stop = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
};

// Initialization of keys
Expand All @@ -37,27 +38,23 @@ class laser_goal : public BT::StatefulActionNode {
return ports_list;
}

BT::NodeStatus onStart() override;
BT::NodeStatus tick() override;
void halt() override;

BT::NodeStatus onRunning() override;

void onHalted() override;

private:
Pose2D goal_point;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
MoveBaseClient ac;
MoveBaseClient ac_;
bool _aborted;

std_msgs::Float64MultiArray the_best_way;

ros::Subscriber radar_org_data;
std::chrono::system_clock::time_point _completion_time;
void LaserCallback(const std_msgs::Float64MultiArray msg)
{
the_best_way=msg;
};

ros::Publisher pub_stop;
geometry_msgs::Twist vel;

};

}
// laser_goal
// Move_to_targe
72 changes: 72 additions & 0 deletions include/rc_decision/aurora/StopMovement.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
/*
* @Author: robox-xx [email protected]
* @Date: 2023-03-15 11:04:27
* @LastEditors: robox-xx [email protected]
* @LastEditTime: 2023-04-18 15:46:03
*/
#pragma once
#include <ros/ros.h>
#include <iostream>
#include <rc_msgs/IbusData.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <behaviortree_cpp_v3/behavior_tree.h>
#include <behaviortree_cpp_v3/action_node.h>
#include <rc_decision/aurora/Move_to_targe.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
//利用该自定义修饰节点暂停导航
namespace rc_decision {

class StopMovement : public BT::DecoratorNode {
public:
StopMovement (const std::string& name, const BT::NodeConfiguration& config)
: BT::DecoratorNode(name, config)
{
ros::NodeHandle n;
ibus_data_ = n.subscribe("ibus_data", 5, &StopMovement::IbusCallback, this);
};

// 这个必须要写，处理接口参数的
static BT::PortsList providedPorts() {
BT::PortsList ports_list;
//拿出黑板数据的初始化
return ports_list;
}

BT::NodeStatus tick() override
{
// 获取子节点并执行
auto child_node = child();
const auto child_status = child()->executeTick();


// 如果子节点返回FAILURE，则立即中断
if (child_status == BT::NodeStatus::FAILURE)
{
child()->halt();
return BT::NodeStatus::FAILURE;
}
if (child_status == BT::NodeStatus::RUNNING)
{
if(ibus_.sw_d == 2)
{
child()->halt();
return BT::NodeStatus::FAILURE;
}
}

return BT::NodeStatus::SUCCESS;

}
private:
ros::Subscriber ibus_data_;
rc_msgs::IbusData ibus_;
void IbusCallback(const rc_msgs::IbusDataConstPtr& ibus_data)
{
ibus_.sw_d = ibus_data->sw_d;
}

};

}
126 changes: 118 additions & 8 deletions include/rc_decision/blackboard/blackboard.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,14 +1,16 @@
/*
* @Author: robox-xx [email protected]
* @Date: 2023-03-15 11:04:27
* @LastEditors: robox-xx 1399786770@qq.com
* @LastEditTime: 2023-04-03 14:19:33
* @LastEditors: robox-xx [email protected].com
* @LastEditTime: 2023-04-18 16:00:25
* @FilePath: /rc_ws/src/rc_decision/include/rc_decision/blackboard/blackboard.h
*/
#pragma once
#include <rc_decision/bt_action_node.h>
#include <rc_decision/bt_service_node.h>
#include <std_msgs/Float64MultiArray.h>
#include <rc_msgs/IbusData.h>
#include <rc_decision/aurora/movebase_client.h>

namespace rc_decision
{
Expand All @@ -18,22 +20,55 @@ class BlackboardRead : public BT::SyncActionNode
BlackboardRead(const std::string& name, const BT::NodeConfiguration& config): BT::SyncActionNode(name, config)
{
ros::NodeHandle nh;
radar_tf_data_ = nh.subscribe("/map/shootPosition", 5, &BlackboardRead::LadarCallback, this);
//获取雷达数据点
radar_tf_data_ = nh.subscribe("/radar/post_Position", 5, &BlackboardRead::LadarCallback, this);
//获取全场柱子的位置
post_data_ = nh.subscribe("/base_link/post_Position", 5, &BlackboardRead::PostCallback, this);
//获取航模遥控数据
ibus_data_ = nh.subscribe("ibus_data", 5, &BlackboardRead::IbusCallback, this);
};

//Initialization of keys
static BT::PortsList providedPorts() {
BT::PortsList ports_list;
//初始化数值的键
ports_list.insert(BT::OutputPort<Pose2D>("Goal"));

ports_list.insert(BT::OutputPort<Pose2D>("sxy1"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy2"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy3"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy4"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy5"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy6"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy7"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy8"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy9"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy10"));
ports_list.insert(BT::OutputPort<Pose2D>("sxy11"));

ports_list.insert(BT::OutputPort<rc_msgs::IbusData>("Ibus_data"));

return ports_list;
}

//Adding data to the blackboard
BT::NodeStatus tick() override {

setOutput("Goal", goal_);
ROS_INFO("i send x:%f, y:%f", goal_.x,goal_.y);

setOutput("sxy1", sxy1);
setOutput("sxy2", sxy2);
setOutput("sxy3", sxy3);
setOutput("sxy4", sxy4);
setOutput("sxy5", sxy5);
setOutput("sxy6", sxy6);
setOutput("sxy7", sxy7);
setOutput("sxy8", sxy8);
setOutput("sxy9", sxy9);
setOutput("sxy10", sxy10);
setOutput("sxy11", sxy11);

setOutput("Ibus_data", ibus_);
return BT::NodeStatus::SUCCESS;
};

Expand All @@ -43,14 +78,89 @@ class BlackboardRead : public BT::SyncActionNode

ros::Subscriber radar_tf_data_;
ros::Subscriber ibus_data_;
Pose2D goal_;
ros::Subscriber post_data_;

Pose2D goal_;
Pose2D sxy1;
Pose2D sxy2;
Pose2D sxy3;
Pose2D sxy4;
Pose2D sxy5;
Pose2D sxy6;
Pose2D sxy7;
Pose2D sxy8;
Pose2D sxy9;
Pose2D sxy10;
Pose2D sxy11;
rc_msgs::IbusData ibus_;
void LadarCallback(const std_msgs::Float64MultiArray msg)
{
{
goal_.x=msg.data[0];
goal_.y=msg.data[1];
goal_.theta = atan(goal_.x/goal_.y);
}

}

void PostCallback(const std_msgs::Float64MultiArray post_data)
{
sxy1.x = post_data.data[0];
sxy1.y = post_data.data[1];
sxy1.theta = atan(sxy1.x/sxy1.y);

sxy2.x = post_data.data[2];
sxy2.y = post_data.data[3];
sxy2.theta = atan(sxy2.x/sxy2.y);

sxy3.x = post_data.data[4];
sxy3.y = post_data.data[5];
sxy3.theta = atan(sxy3.x/sxy3.y);

sxy4.x = post_data.data[6];
sxy4.y = post_data.data[7];
sxy4.theta = atan(sxy4.x/sxy4.y);

sxy5.x = post_data.data[8];
sxy5.y = post_data.data[9];
sxy5.theta = atan(sxy5.x/sxy5.y);

sxy6.x = post_data.data[10];
sxy6.y = post_data.data[11];
sxy6.theta = atan(sxy6.x/sxy6.y);

sxy7.x = post_data.data[12];
sxy7.y = post_data.data[13];
sxy7.theta = atan(sxy7.x/sxy7.y);

sxy8.x = post_data.data[14];
sxy8.y = post_data.data[15];
sxy8.theta = atan(sxy8.x/sxy8.y);

sxy9.x = post_data.data[16];
sxy9.y = post_data.data[17];
sxy9.theta = atan(sxy9.x/sxy9.y);

sxy10.x = post_data.data[18];
sxy10.y = post_data.data[19];
sxy10.theta = atan(sxy10.x/sxy10.y);

sxy11.x = post_data.data[20];
sxy11.y = post_data.data[21];
sxy11.theta = atan(sxy11.x/sxy11.y);
}

void IbusCallback(const rc_msgs::IbusDataConstPtr& ibus_data)
{
ibus_.sw_a = ibus_data->sw_a;
ibus_.sw_b = ibus_data->sw_b;
ibus_.sw_c = ibus_data->sw_c;
ibus_.sw_d = ibus_data->sw_d;
ibus_.key_l = ibus_data->key_l;
ibus_.key_r = ibus_data->key_r;
ibus_.ch_l_x = ibus_data->ch_l_x;
ibus_.ch_l_y = ibus_data->ch_l_y;
ibus_.ch_r_x = ibus_data->ch_r_x;
ibus_.ch_r_y = ibus_data->ch_r_y;
ibus_.vr_a = ibus_data->vr_a;
ibus_.vr_b = ibus_data->vr_b;
}
};
} //namespace rc_decision
Loading