diff --git a/docker/Makefile b/docker/Makefile index 599d8d2e2..19433f10a 100644 --- a/docker/Makefile +++ b/docker/Makefile @@ -14,6 +14,13 @@ vnc: $(eval IMAGE=ghcr.io/memristor/mep3-vnc) @true +build: + echo ${NO_CACHE_ARG} + @DOCKER_BUILDKIT=1 docker build ${DOCKER_DIR} -f ${DOCKER_DIR}/Dockerfile.base -t mep3 ${DOCKER_ARGS} --build-arg UID=${UID} + @[ ${FLAVOR} != 'devel' ] && \ + DOCKER_BUILDKIT=1 docker build ${DOCKER_DIR} -f ${DOCKER_DIR}/Dockerfile.${FLAVOR} -t ${IMAGE} ${DOCKER_ARGS} || \ + true + run: @${MAKE} -s test-nvidia @docker run \ diff --git a/mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp b/mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp index 2a75c2e2a..d63269a52 100644 --- a/mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp +++ b/mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp @@ -60,10 +60,10 @@ namespace mep3_behavior bool setGoal(Goal &goal) override { - std::cout << "Dynamixel desired position to θ=" << position_ \ - <<" max_velocity="<result); setOutput("feedback_effort", wr.result->last_effort); setOutput("feedback_position", wr.result->last_position); - std::cout << "Last result: " << (double)wr.result->result << "; last effort: " << (double)wr.result->last_effort << "; last position: " << (double)wr.result->last_position << std::endl; + std::cout << "Last result: " << (double)wr.result->result << "; last effort: " << (double)wr.result->last_effort << "; last position: " << (double)wr.result->last_position << std::endl; return BT::NodeStatus::SUCCESS; } @@ -112,7 +112,7 @@ namespace mep3_behavior { setOutput("feedback_effort", feedback->effort); - std::cout << "====Max effort: " << feedback->effort <effort << std::endl; setOutput("feedback_position", feedback->position); return BT::NodeStatus::RUNNING; diff --git a/mep3_behavior/include/mep3_behavior/move_action.hpp b/mep3_behavior/include/mep3_behavior/move_action.hpp index 26784c09a..e2ea7783d 100644 --- a/mep3_behavior/include/mep3_behavior/move_action.hpp +++ b/mep3_behavior/include/mep3_behavior/move_action.hpp @@ -16,6 +16,7 @@ #define MEP3_BEHAVIOR_TREE__MOVE_ACTION_HPP_ #include +#include #include #include "behaviortree_cpp/behavior_tree.h" @@ -23,247 +24,187 @@ #include "mep3_behavior/bt_action_node.hpp" #include "mep3_behavior/pose_2d.hpp" #include "mep3_msgs/action/move.hpp" +#include "mep3_msgs/msg/move_command.hpp" + +using namespace BT; namespace mep3_behavior { - class MoveAction + class TranslateAction : public BT::RosActionNode { public: - MoveAction(const std::string &name, - const BT::NodeConfiguration &conf, - const BT::ActionNodeParams ¶ms, - typename std::shared_ptr action_client) - : BT::RosActionNode(name, conf, params, action_client) + TranslateAction(const std::string &name, + const NodeConfig &conf, + const ActionNodeParams ¶ms, + typename std::shared_ptr action_client) + : RosActionNode(name, conf, params, action_client) + { + } + + static BT::PortsList providedPorts() { - if (!getInput("goal", target_pose_)) - throw BT::RuntimeError( - "Move action requires 'goal' argument"); - if (!getInput("ignore_obstacles", ignore_obstacles_)) - ignore_obstacles_=true; - if (!getInput("max_velocity", max_velocity_)) - max_velocity_ = 99999; - if (!getInput("max_acceleration", max_acceleration_)) - max_velocity_ = 99999; - if (!getInput("type", type_)) - type_ = mep3_msgs::action::Move::Goal::TYPE_FULL_NO_REVERSING; - - std::string table = this->config().blackboard->get("table"); - BT::Pose2D goal_offset; - if (table.length() > 0 && getInput("goal_" + table, goal_offset)) { - target_pose_ += goal_offset; - } - - BT::TeamColor color = this->config().blackboard->get("color"); - if (color == BT::TeamColor::GREEN) { - target_pose_ = BT::mirrorPose(target_pose_); - } + return {BT::InputPort("x"), + BT::InputPort("linear_velocity"), + BT::InputPort("frame_id"), + BT::InputPort("ignore_obstacles"), + BT::InputPort("reversing"), + BT::OutputPort("error")}; } bool setGoal(Goal &goal) override { - std::cout << "Move to x=" << target_pose_.x \ - << " y=" << target_pose_.y \ - << " θ=" << target_pose_.theta << "°"\ - <<" max_velocity="<("x", goal.target.x); + getInput("linear_velocity", goal.linear_properties.max_velocity); + getInput("frame_id", goal.header.frame_id); + getInput("ignore_obstacles", goal.ignore_obstacles); + + std::cout << "TranslateAction: setGoal" << std::endl; + std::cout << " x: " << goal.target.x << std::endl; + std::cout << " frame_id: " << goal.header.frame_id << std::endl; + std::cout << " ignore_obstacles: " << goal.ignore_obstacles << std::endl; + std::cout << " linear_velocity: " << goal.linear_properties.max_velocity << std::endl; + + goal.mode = mep3_msgs::msg::MoveCommand::MODE_TRANSLATE; return true; } - static BT::PortsList providedPorts() + BT::NodeStatus onResultReceived(const WrappedResult &wr) override { - // Static parameters - BT::PortsList port_list = { - BT::InputPort("goal"), - BT::InputPort("ignore_obstacles"), - BT::InputPort("max_velocity"), - BT::InputPort("max_acceleration"), - BT::InputPort("type") - }; - - // Dynamic parameters - for (std::string table : BT::SharedBlackboard::access()->get>("predefined_tables")) - { - port_list.insert( - BT::InputPort("goal_" + table)); - } - return port_list; - } + RCLCPP_INFO(node_->get_logger(), "%s: onResultReceived %d", name().c_str(), wr.result->error); - BT::NodeStatus onResultReceived(const WrappedResult & /*wr*/) override - { - return BT::NodeStatus::SUCCESS; - } + setOutput("error", wr.result->error); - private: - BT::Pose2D target_pose_; - bool ignore_obstacles_; - double max_velocity_; - double max_acceleration_; - int type_; + return wr.result->error ? NodeStatus::FAILURE : NodeStatus::SUCCESS; + } }; - - - class TranslateAction + class RotateAction : public BT::RosActionNode { public: - TranslateAction(const std::string &name, - const BT::NodeConfiguration &conf, - const BT::ActionNodeParams ¶ms, - typename std::shared_ptr action_client) - : BT::RosActionNode(name, conf, params, action_client) + RotateAction(const std::string &name, + const NodeConfig &conf, + const ActionNodeParams ¶ms, + typename std::shared_ptr action_client) + : RosActionNode(name, conf, params, action_client) { - if (!getInput("goal", target_position_)) - throw BT::RuntimeError( - "Move action requires 'goal' argument" - ); - if (!getInput("max_velocity", max_velocity_)) - max_velocity_ = 99999; - // ala ce robot da leti :) - if (!getInput("max_acceleration", max_acceleration_)) - max_velocity_ = 99999; - if (!getInput("ignore_obstacles", ignore_obstacles_)) - ignore_obstacles_=true; - - std::string table = this->config().blackboard->get("table"); - double goal_offset; - if (table.length() > 0 && getInput("goal_" + table, goal_offset)) { - target_position_ += goal_offset; - } + } + static BT::PortsList providedPorts() + { + return { + BT::InputPort("angle"), + BT::InputPort("angular_velocity"), + BT::InputPort("frame_id"), + BT::InputPort("ignore_obstacles"), + BT::OutputPort("error")}; } bool setGoal(Goal &goal) override { - std::cout << "Translate to " << target_position_ \ - << "m max_velocity="<("angle", yaw_deg); + getInput("frame_id", goal.header.frame_id); + getInput("ignore_obstacles", goal.ignore_obstacles); + getInput("angular_velocity", goal.angular_properties.max_velocity); + goal.target.theta = yaw_deg * M_PI / 180.0; + + std::cout << "RotateAction: setGoal" << std::endl; + std::cout << " angle: " << goal.target.theta << std::endl; + std::cout << " frame_id: " << goal.header.frame_id << std::endl; + std::cout << " angular_velocity: " << goal.angular_properties.max_velocity << std::endl; + std::cout << " ignore_obstacles: " << goal.ignore_obstacles << std::endl; + + goal.mode = mep3_msgs::msg::MoveCommand::MODE_ROTATE_AT_GOAL; return true; } - static BT::PortsList providedPorts() + BT::NodeStatus onResultReceived(const WrappedResult &wr) override { - // Static parameters - BT::PortsList port_list = { - BT::InputPort("goal"), - BT::InputPort("max_velocity"), - BT::InputPort("max_acceleration"), - BT::InputPort("ignore_obstacles")}; - - // Dynamic parameters - for (std::string table : BT::SharedBlackboard::access()->get>("predefined_tables")) - { - port_list.insert( - BT::InputPort("goal_" + table)); - } - return port_list; - } + RCLCPP_INFO(node_->get_logger(), "%s: onResultReceived %d", name().c_str(), wr.result->error); - BT::NodeStatus onResultReceived(const WrappedResult & /*wr*/) override - { - return BT::NodeStatus::SUCCESS; - } + setOutput("error", wr.result->error); - private: - double target_position_; - double max_velocity_; - double max_acceleration_; - bool ignore_obstacles_; + return wr.result->error ? NodeStatus::FAILURE : NodeStatus::SUCCESS; + } }; - - - -class RotateAction + class MoveAction : public BT::RosActionNode { public: - RotateAction(const std::string &name, - const BT::NodeConfiguration &conf, - const BT::ActionNodeParams ¶ms, - typename std::shared_ptr action_client) - : BT::RosActionNode(name, conf, params, action_client) + MoveAction(const std::string &name, + const NodeConfig &conf, + const ActionNodeParams ¶ms, + typename std::shared_ptr action_client) + : RosActionNode(name, conf, params, action_client) { - if (!getInput("angle", target_angle_)) - throw BT::RuntimeError( - "Move action requires 'goal' argument" - ); - if (!getInput("max_velocity", max_velocity_)) - max_velocity_ = 99999; - - std::string table = this->config().blackboard->get("table"); - double goal_offset; - if (table.length() > 0 && getInput("angle_" + table, goal_offset)) { - target_angle_ += goal_offset; - } + } + static BT::PortsList providedPorts() + { + return { + BT::InputPort("goal"), + BT::InputPort("linear_velocity"), + BT::InputPort("angular_velocity"), + BT::InputPort("frame_id"), + BT::InputPort("ignore_obstacles"), + BT::InputPort("mode"), + BT::OutputPort("error")}; } bool setGoal(Goal &goal) override { - std::cout << "Rotate to θ=" << target_angle_ << "°"\ - << " max_velocity="<("goal", position); + getInput("frame_id", goal.header.frame_id); + getInput("ignore_obstacles", goal.ignore_obstacles); + getInput("linear_velocity", goal.linear_properties.max_velocity); + getInput("angular_velocity", goal.angular_properties.max_velocity); + getInput("mode", mode); + + std::istringstream iss(position); + std::getline(iss, token, ';'); + goal.target.x = std::stod(token); + + std::getline(iss, token, ';'); + goal.target.y = std::stod(token); + + std::getline(iss, token, ';'); + goal.target.theta = std::stod(token) * M_PI / 180.0; + goal.mode = mode; + + std::cout << "RotateAction: setGoal" << std::endl; + std::cout << " x: " << goal.target.x << std::endl; + std::cout << " y: " << goal.target.y << std::endl; + std::cout << " angle: " << goal.target.theta << std::endl; + std::cout << " mode: " << goal.mode << "==" << mode << std::endl; + std::cout << " frame_id: " << goal.header.frame_id << std::endl; + std::cout << " linear_velocity: " << goal.linear_properties.max_velocity << std::endl; + std::cout << " anguar_velocity: " << goal.angular_properties.max_velocity << std::endl; + std::cout << " ignore_obstacles: " << goal.ignore_obstacles << std::endl; return true; } - static BT::PortsList providedPorts() + BT::NodeStatus onResultReceived(const WrappedResult &wr) override { - // Static parameters - BT::PortsList port_list = { - BT::InputPort("angle"), - BT::InputPort("max_velocity")}; - - // Dynamic parameters - for (std::string table : BT::SharedBlackboard::access()->get>("predefined_tables")) - { - port_list.insert( - BT::InputPort("angle_" + table)); - } - return port_list; - } + RCLCPP_INFO(node_->get_logger(), "%s: onResultReceived %d", name().c_str(), wr.result->error); - BT::NodeStatus onResultReceived(const WrappedResult & /*wr*/) override - { - return BT::NodeStatus::SUCCESS; - } + setOutput("error", wr.result->error); - private: - double target_angle_; - double max_velocity_; + return wr.result->error ? NodeStatus::FAILURE : NodeStatus::SUCCESS; + } }; - } // namespace mep3_behavior #endif // MEP3_BEHAVIOR_TREE__MOVE_ACTION_HPP_ diff --git a/mep3_behavior/src/mep3_behavior_tree.cpp b/mep3_behavior/src/mep3_behavior_tree.cpp index b09227e02..6ca7c8f83 100644 --- a/mep3_behavior/src/mep3_behavior_tree.cpp +++ b/mep3_behavior/src/mep3_behavior_tree.cpp @@ -118,13 +118,14 @@ int main(int argc, char **argv) node->declare_parameter("color", "blue"); auto color = node->get_parameter("color").as_string(); - //Adapt different start field to default color + // Adapt different start field to default color size_t found = color.find('_'); - if(found != std::string::npos){ + if (found != std::string::npos) + { color.erase(found, color.size()); } - - std::cout<<"ERASED COLOR: "<set("color", BT::TeamColor::GREEN); @@ -138,12 +139,13 @@ int main(int argc, char **argv) "SetSharedBlackboard"); factory.registerNodeType( "Wait"); + BT::RegisterRosAction(factory, "JointPosition", {node, "joint_position_command", std::chrono::seconds(30)}); - BT::RegisterRosAction(factory, "Move", {node, "move", std::chrono::seconds(30)}); BT::RegisterRosAction(factory, "Navigate", {node, "navigate_to_pose", std::chrono::seconds(30)}); BT::RegisterRosAction(factory, "Pump", {node, "pump", std::chrono::seconds(30)}); - BT::RegisterRosAction(factory, "Translate", {node, "move", std::chrono::seconds(30)}); - BT::RegisterRosAction(factory, "Rotate", {node, "move", std::chrono::seconds(30)}); + BT::RegisterRosAction(factory, "Translate", {node, "move/move", std::chrono::seconds(30)}); + BT::RegisterRosAction(factory, "Rotate", {node, "move/move", std::chrono::seconds(30)}); + BT::RegisterRosAction(factory, "Move", {node, "move/move", std::chrono::seconds(30)}); factory.registerNodeType( "ScoreboardTask"); diff --git a/mep3_behavior/strategies/big_blue.xml b/mep3_behavior/strategies/big_blue.xml deleted file mode 100644 index 07a356c24..000000000 --- a/mep3_behavior/strategies/big_blue.xml +++ /dev/null @@ -1,125 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/big_homologation.xml b/mep3_behavior/strategies/big_homologation.xml deleted file mode 100644 index 9faa2ba0f..000000000 --- a/mep3_behavior/strategies/big_homologation.xml +++ /dev/null @@ -1,111 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/big_strategy.xml b/mep3_behavior/strategies/big_strategy.xml deleted file mode 100644 index 294834c02..000000000 --- a/mep3_behavior/strategies/big_strategy.xml +++ /dev/null @@ -1,293 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/big_strategy_a.xml b/mep3_behavior/strategies/big_strategy_a.xml deleted file mode 100644 index 49a3a5c8d..000000000 --- a/mep3_behavior/strategies/big_strategy_a.xml +++ /dev/null @@ -1,294 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/big_strategy_demo.xml b/mep3_behavior/strategies/big_strategy_demo.xml deleted file mode 100644 index 8607e3beb..000000000 --- a/mep3_behavior/strategies/big_strategy_demo.xml +++ /dev/null @@ -1,99 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Key - Value - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/big_strategy_green.xml b/mep3_behavior/strategies/big_strategy_green.xml deleted file mode 100644 index 76af2bffb..000000000 --- a/mep3_behavior/strategies/big_strategy_green.xml +++ /dev/null @@ -1,276 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/big_strategy_green_a.xml b/mep3_behavior/strategies/big_strategy_green_a.xml deleted file mode 100644 index 6ba9aa553..000000000 --- a/mep3_behavior/strategies/big_strategy_green_a.xml +++ /dev/null @@ -1,284 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/big_strategy_green_s.xml b/mep3_behavior/strategies/big_strategy_green_s.xml deleted file mode 100644 index b5c3e5160..000000000 --- a/mep3_behavior/strategies/big_strategy_green_s.xml +++ /dev/null @@ -1,267 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/big_strategy_s.xml b/mep3_behavior/strategies/big_strategy_s.xml deleted file mode 100644 index b8c1c348b..000000000 --- a/mep3_behavior/strategies/big_strategy_s.xml +++ /dev/null @@ -1,276 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/big_strategy_simple.xml b/mep3_behavior/strategies/big_strategy_simple.xml deleted file mode 100644 index 69f0f3325..000000000 --- a/mep3_behavior/strategies/big_strategy_simple.xml +++ /dev/null @@ -1,353 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/big_strategy_simple_a.xml b/mep3_behavior/strategies/big_strategy_simple_a.xml deleted file mode 100644 index bd2fd4097..000000000 --- a/mep3_behavior/strategies/big_strategy_simple_a.xml +++ /dev/null @@ -1,351 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/big_strategy_simple_green.xml b/mep3_behavior/strategies/big_strategy_simple_green.xml deleted file mode 100644 index 9c3c6a2e2..000000000 --- a/mep3_behavior/strategies/big_strategy_simple_green.xml +++ /dev/null @@ -1,342 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/big_strategy_simple_green_a.xml b/mep3_behavior/strategies/big_strategy_simple_green_a.xml deleted file mode 100644 index 2b6b39beb..000000000 --- a/mep3_behavior/strategies/big_strategy_simple_green_a.xml +++ /dev/null @@ -1,340 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/big_strategy_simple_green_s.xml b/mep3_behavior/strategies/big_strategy_simple_green_s.xml deleted file mode 100644 index b734071b3..000000000 --- a/mep3_behavior/strategies/big_strategy_simple_green_s.xml +++ /dev/null @@ -1,416 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/big_strategy_simple_s.xml b/mep3_behavior/strategies/big_strategy_simple_s.xml deleted file mode 100644 index 62864b453..000000000 --- a/mep3_behavior/strategies/big_strategy_simple_s.xml +++ /dev/null @@ -1,417 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/common.xml b/mep3_behavior/strategies/common.xml deleted file mode 100644 index 47fd8b9f4..000000000 --- a/mep3_behavior/strategies/common.xml +++ /dev/null @@ -1,86 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/detection.xml b/mep3_behavior/strategies/detection.xml deleted file mode 100644 index 4b06c6571..000000000 --- a/mep3_behavior/strategies/detection.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/duvanje.xml b/mep3_behavior/strategies/duvanje.xml deleted file mode 100644 index 26395b1e9..000000000 --- a/mep3_behavior/strategies/duvanje.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - diff --git a/mep3_behavior/strategies/funny_blue.xml b/mep3_behavior/strategies/funny_blue.xml deleted file mode 100644 index bedab4731..000000000 --- a/mep3_behavior/strategies/funny_blue.xml +++ /dev/null @@ -1,138 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/funny_green.xml b/mep3_behavior/strategies/funny_green.xml deleted file mode 100644 index 0a4a78810..000000000 --- a/mep3_behavior/strategies/funny_green.xml +++ /dev/null @@ -1,138 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/init.xml b/mep3_behavior/strategies/init.xml new file mode 100644 index 000000000..36e9b9d0b --- /dev/null +++ b/mep3_behavior/strategies/init.xml @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/move_cherry.xml b/mep3_behavior/strategies/move_cherry.xml deleted file mode 100644 index e58891362..000000000 --- a/mep3_behavior/strategies/move_cherry.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/move_homologation.xml b/mep3_behavior/strategies/move_homologation.xml deleted file mode 100644 index ab67f68d4..000000000 --- a/mep3_behavior/strategies/move_homologation.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/move_wait.xml b/mep3_behavior/strategies/move_wait.xml deleted file mode 100644 index 9a09be61e..000000000 --- a/mep3_behavior/strategies/move_wait.xml +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/gentle_attack.xml b/mep3_behavior/strategies/old_strategy/common.xml similarity index 100% rename from mep3_behavior/strategies/gentle_attack.xml rename to mep3_behavior/strategies/old_strategy/common.xml diff --git a/mep3_behavior/strategies/effort_test.xml b/mep3_behavior/strategies/old_strategy/effort_test.xml similarity index 100% rename from mep3_behavior/strategies/effort_test.xml rename to mep3_behavior/strategies/old_strategy/effort_test.xml diff --git a/mep3_behavior/strategies/izduvavanje.xml b/mep3_behavior/strategies/old_strategy/gentle_attack.xml similarity index 50% rename from mep3_behavior/strategies/izduvavanje.xml rename to mep3_behavior/strategies/old_strategy/gentle_attack.xml index 0e0b09f46..3a14b9468 100644 --- a/mep3_behavior/strategies/izduvavanje.xml +++ b/mep3_behavior/strategies/old_strategy/gentle_attack.xml @@ -1,16 +1,23 @@ - + - + - - + max_velocity="1.0" + type="0"/> + + + + + @@ -32,6 +39,17 @@ + + + + + + diff --git a/mep3_behavior/strategies/live.xml b/mep3_behavior/strategies/old_strategy/live.xml similarity index 100% rename from mep3_behavior/strategies/live.xml rename to mep3_behavior/strategies/old_strategy/live.xml diff --git a/mep3_behavior/strategies/prepair_pick_cake.xml b/mep3_behavior/strategies/prepair_pick_cake.xml deleted file mode 100644 index 775880ac5..000000000 --- a/mep3_behavior/strategies/prepair_pick_cake.xml +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/project.btproj b/mep3_behavior/strategies/project.btproj index fb611c6e7..8d8f8adc6 100644 --- a/mep3_behavior/strategies/project.btproj +++ b/mep3_behavior/strategies/project.btproj @@ -1,63 +1,8 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + @@ -75,16 +20,17 @@ - - - + + + - - - - + + + + + @@ -99,7 +45,10 @@ - + + + + points scored, can be negative @@ -111,10 +60,11 @@ - + - - + + + diff --git a/mep3_behavior/strategies/skill_basket_blue.xml b/mep3_behavior/strategies/skill_basket_blue.xml deleted file mode 100644 index caf4f7e2f..000000000 --- a/mep3_behavior/strategies/skill_basket_blue.xml +++ /dev/null @@ -1,117 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_basket_green.xml b/mep3_behavior/strategies/skill_basket_green.xml deleted file mode 100644 index 7583b1361..000000000 --- a/mep3_behavior/strategies/skill_basket_green.xml +++ /dev/null @@ -1,117 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_drop_cake.xml b/mep3_behavior/strategies/skill_drop_cake.xml deleted file mode 100644 index b4d54af6d..000000000 --- a/mep3_behavior/strategies/skill_drop_cake.xml +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_drop_cake_right.xml b/mep3_behavior/strategies/skill_drop_cake_right.xml deleted file mode 100644 index d7d52fa46..000000000 --- a/mep3_behavior/strategies/skill_drop_cake_right.xml +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_green_start.xml b/mep3_behavior/strategies/skill_green_start.xml deleted file mode 100644 index 715ec4c08..000000000 --- a/mep3_behavior/strategies/skill_green_start.xml +++ /dev/null @@ -1,69 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - points scored, can be negative - unique task name - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/skill_lift_cake.xml b/mep3_behavior/strategies/skill_lift_cake.xml deleted file mode 100644 index 0320949eb..000000000 --- a/mep3_behavior/strategies/skill_lift_cake.xml +++ /dev/null @@ -1,72 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_lift_cake_right.xml b/mep3_behavior/strategies/skill_lift_cake_right.xml deleted file mode 100644 index 262a125bc..000000000 --- a/mep3_behavior/strategies/skill_lift_cake_right.xml +++ /dev/null @@ -1,72 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_pick_cake.xml b/mep3_behavior/strategies/skill_pick_cake.xml deleted file mode 100644 index 6a9e2fb31..000000000 --- a/mep3_behavior/strategies/skill_pick_cake.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_pick_cherry.xml b/mep3_behavior/strategies/skill_pick_cherry.xml deleted file mode 100644 index 2ae357658..000000000 --- a/mep3_behavior/strategies/skill_pick_cherry.xml +++ /dev/null @@ -1,123 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_pick_one_cherry.xml b/mep3_behavior/strategies/skill_pick_one_cherry.xml deleted file mode 100644 index 5ec6f7966..000000000 --- a/mep3_behavior/strategies/skill_pick_one_cherry.xml +++ /dev/null @@ -1,91 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_pick_two.xml b/mep3_behavior/strategies/skill_pick_two.xml deleted file mode 100644 index 8e20c47a5..000000000 --- a/mep3_behavior/strategies/skill_pick_two.xml +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_put_cherry.xml b/mep3_behavior/strategies/skill_put_cherry.xml deleted file mode 100644 index 175fd7f98..000000000 --- a/mep3_behavior/strategies/skill_put_cherry.xml +++ /dev/null @@ -1,77 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_put_cherry_right.xml b/mep3_behavior/strategies/skill_put_cherry_right.xml deleted file mode 100644 index 6e27f09e9..000000000 --- a/mep3_behavior/strategies/skill_put_cherry_right.xml +++ /dev/null @@ -1,77 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_put_cherry_up.xml b/mep3_behavior/strategies/skill_put_cherry_up.xml deleted file mode 100644 index 574de3746..000000000 --- a/mep3_behavior/strategies/skill_put_cherry_up.xml +++ /dev/null @@ -1,69 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_put_one_cherry.xml b/mep3_behavior/strategies/skill_put_one_cherry.xml deleted file mode 100644 index da8185014..000000000 --- a/mep3_behavior/strategies/skill_put_one_cherry.xml +++ /dev/null @@ -1,77 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_take_cherry_blue_start.xml b/mep3_behavior/strategies/skill_take_cherry_blue_start.xml deleted file mode 100644 index 01bd284e4..000000000 --- a/mep3_behavior/strategies/skill_take_cherry_blue_start.xml +++ /dev/null @@ -1,88 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_take_cherry_green_start.xml b/mep3_behavior/strategies/skill_take_cherry_green_start.xml deleted file mode 100644 index b70625c5e..000000000 --- a/mep3_behavior/strategies/skill_take_cherry_green_start.xml +++ /dev/null @@ -1,91 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_take_side_cherry.xml b/mep3_behavior/strategies/skill_take_side_cherry.xml deleted file mode 100644 index 7dc3cf143..000000000 --- a/mep3_behavior/strategies/skill_take_side_cherry.xml +++ /dev/null @@ -1,95 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/skill_take_side_cherry_green.xml b/mep3_behavior/strategies/skill_take_side_cherry_green.xml deleted file mode 100644 index b5d2689c0..000000000 --- a/mep3_behavior/strategies/skill_take_side_cherry_green.xml +++ /dev/null @@ -1,112 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/small_blue_simple.xml b/mep3_behavior/strategies/small_blue_simple.xml deleted file mode 100644 index 47b30288b..000000000 --- a/mep3_behavior/strategies/small_blue_simple.xml +++ /dev/null @@ -1,153 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/small_blue_simple_s.xml b/mep3_behavior/strategies/small_blue_simple_s.xml deleted file mode 100644 index d903f2b45..000000000 --- a/mep3_behavior/strategies/small_blue_simple_s.xml +++ /dev/null @@ -1,174 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/small_green_simple.xml b/mep3_behavior/strategies/small_green_simple.xml deleted file mode 100644 index c05d25dcb..000000000 --- a/mep3_behavior/strategies/small_green_simple.xml +++ /dev/null @@ -1,153 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/small_green_simple_s.xml b/mep3_behavior/strategies/small_green_simple_s.xml deleted file mode 100644 index 5bdac9123..000000000 --- a/mep3_behavior/strategies/small_green_simple_s.xml +++ /dev/null @@ -1,173 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/small_homologation.xml b/mep3_behavior/strategies/small_homologation.xml deleted file mode 100644 index a25cb0cb5..000000000 --- a/mep3_behavior/strategies/small_homologation.xml +++ /dev/null @@ -1,77 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/small_strategy.xml b/mep3_behavior/strategies/small_strategy.xml deleted file mode 100644 index 2bdc7a729..000000000 --- a/mep3_behavior/strategies/small_strategy.xml +++ /dev/null @@ -1,146 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/small_strategy_demo.xml b/mep3_behavior/strategies/small_strategy_demo.xml deleted file mode 100644 index 3bbaa1318..000000000 --- a/mep3_behavior/strategies/small_strategy_demo.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/small_strategy_green.xml b/mep3_behavior/strategies/small_strategy_green.xml deleted file mode 100644 index 9cdd7a87a..000000000 --- a/mep3_behavior/strategies/small_strategy_green.xml +++ /dev/null @@ -1,146 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - For example, 0x12345 - For example, 0x32;0x34 - - - - - - - - - - - - - - - - - points scored, can be negative - unique task name - - - - - - - - - - - - - 0 = unarmed; 1 = armed; 2 = started - - - - diff --git a/mep3_behavior/strategies/test_demo_subtree.xml b/mep3_behavior/strategies/test_demo_subtree.xml deleted file mode 100644 index 46f2837ee..000000000 --- a/mep3_behavior/strategies/test_demo_subtree.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/test_init_servo.xml b/mep3_behavior/strategies/test_init_servo.xml deleted file mode 100644 index 72fab7425..000000000 --- a/mep3_behavior/strategies/test_init_servo.xml +++ /dev/null @@ -1,107 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/test_servo.xml b/mep3_behavior/strategies/test_servo.xml deleted file mode 100644 index b792f5f8b..000000000 --- a/mep3_behavior/strategies/test_servo.xml +++ /dev/null @@ -1,192 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mep3_behavior/strategies/try_move_weed.xml b/mep3_behavior/strategies/try_move_weed.xml new file mode 100644 index 000000000..f5de3a9ca --- /dev/null +++ b/mep3_behavior/strategies/try_move_weed.xml @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/try_pickup_weed.xml b/mep3_behavior/strategies/try_pickup_weed.xml new file mode 100644 index 000000000..e639cfea4 --- /dev/null +++ b/mep3_behavior/strategies/try_pickup_weed.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_behavior/strategies/try_translate.xml b/mep3_behavior/strategies/try_translate.xml index 7c2abbad2..f0d1dce13 100644 --- a/mep3_behavior/strategies/try_translate.xml +++ b/mep3_behavior/strategies/try_translate.xml @@ -1,20 +1,47 @@ + + + + + + - - - - - - - - - + + + + + + + + + + + + + @@ -23,18 +50,33 @@ - - - - + + + + + + + - - + + + + + diff --git a/mep3_bringup/launch/robot_launch.py b/mep3_bringup/launch/robot_launch.py index 636028d47..a75089679 100644 --- a/mep3_bringup/launch/robot_launch.py +++ b/mep3_bringup/launch/robot_launch.py @@ -21,8 +21,8 @@ INITIAL_POSE_MATRIX = [ - ('big', 'blue', [-0.65, -0.43, 0]), - ('small', 'blue', [0.80, 1.34, pi/2]) + ('big', 'blue', [0.03, -1.15, pi/2]), + ('small', 'blue', [0.72, -1.16, pi/2]) ] PREDEFINED_TABLE_NAMES = [ 'table1', diff --git a/mep3_bringup/launch/simulation_launch.py b/mep3_bringup/launch/simulation_launch.py index 112e5d168..7571ae254 100644 --- a/mep3_bringup/launch/simulation_launch.py +++ b/mep3_bringup/launch/simulation_launch.py @@ -10,13 +10,14 @@ from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration - +from launch_ros.actions import Node def generate_launch_description(): use_behavior_tree = LaunchConfiguration('bt', default=True) - big_strategy = LaunchConfiguration('big_strategy', default='big_strategy_demo') - small_strategy = LaunchConfiguration('small_strategy', default='small_strategy_demo') + big_strategy = LaunchConfiguration('big_strategy', default='try_translate') + small_strategy = LaunchConfiguration('small_strategy', default='wait_everytime') color = LaunchConfiguration('color', default='blue') + namespace = LaunchConfiguration('namespace', default='big') use_opponents = LaunchConfiguration('opponents', default=False) debug = LaunchConfiguration('debug', default=False) use_localization = LaunchConfiguration('localization', default=False) @@ -65,13 +66,33 @@ def generate_launch_description(): condition=IfCondition(use_localization), ) + move = Node( + package='mep3_navigation', + executable='move', + output='screen', + parameters=[ + { + 'use_sim_time': False, + # 'angular.max_velocity': 0.5, + # 'angular.max_acceleration': 0.3, + 'angular.tolerance': 0.001, + 'update_rate': 100, + } + ], + namespace=namespace, + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ] + ) + return launch.LaunchDescription([ big_robot, small_robot, - + move, # The easiest way to get pass variables to Webots controllers # is to use environment variables. - localization, + # localization, set_color_action, set_use_opponents, simulation, diff --git a/mep3_bringup/mep3_bringup/launch_utils.py b/mep3_bringup/mep3_bringup/launch_utils.py index 68d397f84..d607a2b80 100644 --- a/mep3_bringup/mep3_bringup/launch_utils.py +++ b/mep3_bringup/mep3_bringup/launch_utils.py @@ -17,7 +17,6 @@ def get_controller_spawners(controller_params_file): controller_names = list(controller_params[item]['ros__parameters'].keys()) break if namespace == '' and 'controller_manager' not in controller_params.keys(): - print(controller_params.keys()) namespace = list(controller_params.keys())[0] controller_names = list(controller_params[namespace]['controller_manager']['ros__parameters'].keys()) if namespace == '': diff --git a/mep3_hardware/launch/hardware_launch.py b/mep3_hardware/launch/hardware_launch.py index fffedd152..906edcb2f 100644 --- a/mep3_hardware/launch/hardware_launch.py +++ b/mep3_hardware/launch/hardware_launch.py @@ -35,6 +35,8 @@ def launch_setup(context, *args, **kwargs): controller_params_file = os.path.join(package_dir, 'resource', f'{performed_namespace}_controllers.yaml') robot_description = os.path.join(package_dir, 'resource', f'{performed_namespace}_description.urdf') + with open(robot_description, 'r') as file: + robot_description = file.read() enable_can_interface() @@ -48,7 +50,9 @@ def launch_setup(context, *args, **kwargs): remappings=[ (f'/{performed_namespace}/diffdrive_controller/cmd_vel_unstamped', 'cmd_vel'), (f'/{performed_namespace}/diffdrive_controller/odom', 'odom'), - ('/tf', 'tf') + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ], namespace=namespace, ros_arguments=['--log-level', 'warn'], @@ -76,7 +80,8 @@ def launch_setup(context, *args, **kwargs): name='rplidar_ros', parameters=[{ 'frame_id': 'laser', - 'serial_port': usb_port + 'serial_port': usb_port, + 'serial_baudrate': 115200, }], ros_arguments=['--log-level', 'warn'], output='screen', @@ -99,14 +104,27 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(PythonExpression(["'", namespace, "' == 'big'"])) ) + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[ + {'robot_description': robot_description}, + ], + namespace=namespace, + ros_arguments=['--log-level', 'warn'], + ) + return [ controller_manager_node, socketcan_bridge, - cinch_driver, + robot_state_publisher, + # cinch_driver, lidar_rplidar, - lcd_driver, - box_driver, - ] + get_controller_spawners(controller_params_file) + # lidar + # lcd_driver, + # box_driver, + ] + get_controller_spawners(controller_params_file) def generate_launch_description(): diff --git a/mep3_hardware/resource/big_controllers.yaml b/mep3_hardware/resource/big_controllers.yaml index 610304af2..92010b51d 100644 --- a/mep3_hardware/resource/big_controllers.yaml +++ b/mep3_hardware/resource/big_controllers.yaml @@ -9,8 +9,6 @@ big: joint_position_controller: type: mep3_controllers/JointPositionController - pump_controller: - type: mep3_controllers/PumpController joint_position_controller: ros__parameters: @@ -18,17 +16,8 @@ big: - m1 - m2 - m3 - - m5 - - m6 - - m7 - - m8 - - m9 - - m10 - - pump_controller: - ros__parameters: - pumps: - - pump1 + - m4 + diffdrive_controller: ros__parameters: @@ -38,7 +27,7 @@ big: odom_frame_id: odom base_frame_id: base_link - wheel_separation: 0.3176 + wheel_separation: 0.288 wheel_radius: 0.04075 right_wheel_radius_multiplier: 1.0 diff --git a/mep3_hardware/resource/big_description.urdf b/mep3_hardware/resource/big_description.urdf index 3dfbd6e17..65df2ace0 100644 --- a/mep3_hardware/resource/big_description.urdf +++ b/mep3_hardware/resource/big_description.urdf @@ -1,5 +1,7 @@ + + mep3_hardware::MotionHardwareInterface @@ -22,40 +24,16 @@ - - - mep3_hardware::CanGpioHardwareInterface - can0 - 0x00006C01 - 0x1FFFFFF8 - - - 0 - - - - 6 - - - dynamixel_hardware/DynamixelHardware - /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A9077OYV-if00-port0 + /dev/serial/by-path/pci-0000:00:14.0-usb-0:3:1.0-port0 115200 false 3.14159 - - 9 - - - - - - - - 6 + + 1 @@ -63,69 +41,39 @@ + dynamixel_hardware/DynamixelHardware - /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0 + /dev/serial/by-path/pci-0000:00:14.0-usb-0:2:1.0-port0 115200 false 2.618 - - 3 - - - - - - - - 7 - - - - - - - - 8 - - - - - - - - 14 - - - - - - - - 55 + + 2 - - 5 + + 3 - - 1 + + 4 + - \ No newline at end of file + diff --git a/mep3_hardware/resource/small_controllers.yaml b/mep3_hardware/resource/small_controllers.yaml index 0448f8765..7250238db 100644 --- a/mep3_hardware/resource/small_controllers.yaml +++ b/mep3_hardware/resource/small_controllers.yaml @@ -5,13 +5,6 @@ small: diffdrive_controller: type: diff_drive_controller/DiffDriveController - joint_position_controller: - type: mep3_controllers/JointPositionController - - joint_position_controller: - ros__parameters: - joints: - - k1 diffdrive_controller: ros__parameters: diff --git a/mep3_hardware/resource/small_description.urdf b/mep3_hardware/resource/small_description.urdf index 6703cf32a..c9e817b0c 100644 --- a/mep3_hardware/resource/small_description.urdf +++ b/mep3_hardware/resource/small_description.urdf @@ -22,37 +22,4 @@ - - - dynamixel_hardware/DynamixelHardware - /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0 - 115200 - false - 2.618 - - - 69 - - - - - - - - 2 - - - - - - - - 11 - - - - - - - diff --git a/mep3_msgs/CMakeLists.txt b/mep3_msgs/CMakeLists.txt index 1554d00be..0d4628d69 100644 --- a/mep3_msgs/CMakeLists.txt +++ b/mep3_msgs/CMakeLists.txt @@ -25,7 +25,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msgs/Scoreboard.msg" "action/VacuumPumpCommand.action" "msgs/TemporalObstacle.msg" - "msgs/MotionProperties.msg" + "msgs/MoveProperties.msg" + "msgs/MoveCommand.msg" + "msgs/MoveState.msg" "action/Move.action" DEPENDENCIES geometry_msgs ) diff --git a/mep3_msgs/action/Move.action b/mep3_msgs/action/Move.action index 24152dc69..139d8095b 100644 --- a/mep3_msgs/action/Move.action +++ b/mep3_msgs/action/Move.action @@ -1,24 +1,17 @@ -uint8 TYPE_FULL_NO_REVERSING = 0 -uint8 TYPE_FULL_AUTO_REVERSING = 4 -uint8 TYPE_FULL_FORCE_REVERSING = 5 -uint8 TYPE_ROTATE = 1 -uint8 TYPE_TRANSLATE = 2 -uint8 TYPE_SKIP_FINAL_ROTATION = 3 - -uint8 RESULT_SUCCESS = 0 -uint8 RESULT_TIMEOUT = 1 -uint8 RESULT_STUCK = 2 -uint8 RESULT_ERROR = 3 - std_msgs/Header header -geometry_msgs/Pose2D target -mep3_msgs/MotionProperties linear_properties -mep3_msgs/MotionProperties angular_properties string odom_frame + +geometry_msgs/Pose2D target + +mep3_msgs/MoveProperties linear_properties +mep3_msgs/MoveProperties angular_properties bool ignore_obstacles -uint8 type builtin_interfaces/Duration timeout +uint8 reversing +uint8 mode 7 + +# TODO +float64 translate_yaw_error_tolerance --- -builtin_interfaces/Duration total_elapsed_time -uint8 result +uint8 error --- diff --git a/mep3_msgs/msgs/MoveCommand.msg b/mep3_msgs/msgs/MoveCommand.msg new file mode 100644 index 000000000..a792e53bc --- /dev/null +++ b/mep3_msgs/msgs/MoveCommand.msg @@ -0,0 +1,22 @@ +uint8 REVERSING_NO = 0 +uint8 REVERSING_AUTO = 1 +uint8 REVERSING_FORCE = 2 + +uint8 MODE_ROTATE_TOWARDS_GOAL = 4 +uint8 MODE_TRANSLATE = 2 +uint8 MODE_ROTATE_AT_GOAL = 1 + +std_msgs/Header header +string odom_frame + +geometry_msgs/Pose2D target + +mep3_msgs/MoveProperties linear_properties +mep3_msgs/MoveProperties angular_properties +bool ignore_obstacles +builtin_interfaces/Duration timeout +uint8 reversing +uint8 mode + +# TODO +float64 translate_yaw_error_tolerance diff --git a/mep3_msgs/msgs/MoveProperties.msg b/mep3_msgs/msgs/MoveProperties.msg new file mode 100644 index 000000000..a13d619a3 --- /dev/null +++ b/mep3_msgs/msgs/MoveProperties.msg @@ -0,0 +1,5 @@ +float64 max_velocity +float64 max_acceleration +float64 kp +float64 kd +float64 tolerance diff --git a/mep3_msgs/msgs/MoveState.msg b/mep3_msgs/msgs/MoveState.msg new file mode 100644 index 000000000..b8fe065a5 --- /dev/null +++ b/mep3_msgs/msgs/MoveState.msg @@ -0,0 +1,18 @@ +uint8 ERROR_NONE = 0 +uint8 ERROR_TIMEOUT = 1 +uint8 ERROR_STUCK = 2 +uint8 ERROR_OBSTACLE = 3 +uint8 ERROR_MISSING_TRANSFORM = 4 + +uint8 STATE_IDLE = 0 +uint8 STATE_ROTATING_TOWARDS_GOAL = 1 +uint8 STATE_TRANSLATING = 2 +uint8 STATE_ROTATING_AT_GOAL = 3 +uint8 STATE_STOPPING = 4 + +uint8 error +uint8 state +float64 distance_x +float64 distance_xy +float64 distance_yaw + diff --git a/mep3_navigation/CMakeLists.txt b/mep3_navigation/CMakeLists.txt index 7e3de407c..4ef73c736 100644 --- a/mep3_navigation/CMakeLists.txt +++ b/mep3_navigation/CMakeLists.txt @@ -113,14 +113,14 @@ install( ) pluginlib_export_plugin_description_file(nav2_costmap_2d dilation_layer.xml) -# move_behavior -add_library( - move_behavior - SHARED - src/move_behavior/move_behavior.cpp +add_executable( + move + src/move/move.cpp + src/move/move_node.cpp ) + ament_target_dependencies( - move_behavior + move rclcpp nav2_costmap_2d pluginlib @@ -129,17 +129,17 @@ ament_target_dependencies( can_msgs ) target_include_directories( - move_behavior + move PRIVATE include ) install( - TARGETS move_behavior - DESTINATION lib + TARGETS move + DESTINATION lib/${PROJECT_NAME} ) -target_link_libraries(move_behavior ruckig::ruckig) -pluginlib_export_plugin_description_file(nav2_core move_behavior.xml) +target_link_libraries(move ruckig::ruckig) +install(TARGETS move DESTINATION lib/${PROJECT_NAME}) # Assets install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) diff --git a/mep3_navigation/launch/navigation_launch.py b/mep3_navigation/launch/navigation_launch.py index 3268d8f0a..9fe893a43 100644 --- a/mep3_navigation/launch/navigation_launch.py +++ b/mep3_navigation/launch/navigation_launch.py @@ -21,6 +21,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode +from launch_ros.actions import Node def generate_launch_description(): @@ -135,6 +136,27 @@ def generate_launch_description(): ], ) + move = Node( + package='mep3_navigation', + executable='move', + output='screen', + parameters=[ + { + 'use_sim_time': use_sim_time, + 'angular.max_velocity': 1.8, + 'angular.max_acceleration': 1.5, + 'angular.tolerance': 0.03, + 'linear.tolerance': 0.01, + 'update_rate': 100, + } + ], + namespace=namespace, + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static') + ] + ) + return LaunchDescription([ stdout_linebuf_envvar, declare_namespace_cmd, @@ -142,5 +164,6 @@ def generate_launch_description(): declare_params_file_cmd, nav2_bt_xml_file_cmd, declare_log_level_cmd, - load_composable_nodes + load_composable_nodes, + move ]) diff --git a/mep3_navigation/move_behavior.xml b/mep3_navigation/move_behavior.xml deleted file mode 100644 index 44e14223e..000000000 --- a/mep3_navigation/move_behavior.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Provides primitive but flexible robot motion behavior. - - \ No newline at end of file diff --git a/mep3_navigation/params/nav2_params_big.yaml b/mep3_navigation/params/nav2_params_big.yaml index 1470a11fb..a953eae26 100644 --- a/mep3_navigation/params/nav2_params_big.yaml +++ b/mep3_navigation/params/nav2_params_big.yaml @@ -148,8 +148,7 @@ plugin: "mep3_navigation::TemporalObstacleLayer" add_obstacle_topic: /big/add_obstacle remove_obstacle_topic: /big/remove_obstacle - predefined_obstacle_labels: ['side_top', 'side_bottom', 'side_left', 'side_right', - 'cherry_dispenser_top', 'cherry_dispenser_bottom', 'cherry_dispenser_left', 'cherry_dispenser_right'] + predefined_obstacle_labels: ['side_top', 'side_bottom', 'side_left', 'side_right'] side_top: '[[1.05, -1.55], [1.05, 1.55], [1.25, 1.55], [1.25, -1.55]]' side_bottom: '[[-1.05, -1.55], [-1.05, 1.55], [-1.25, 1.55], [-1.25, -1.55]]' side_left: '[[1.05, 1.55], [-1.05, 1.55], [-1.05, 1.75], [1.05, 1.75]]' @@ -177,13 +176,11 @@ costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 100.0 - behavior_plugins: ["spin", "wait", "move"] + behavior_plugins: ["spin", "wait"] spin: plugin: "nav2_behaviors/Spin" wait: plugin: "nav2_behaviors/Wait" - move: - plugin: "mep3_navigation::MoveBehavior" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 @@ -192,17 +189,4 @@ min_rotational_vel: 0.4 rotational_acc_lim: 3.2 simulate_ahead_time: 0.3 - - # Move - simulate_ahead_distance: 0.2 - debouncing_duration: 0.05 - linear: - max_velocity: 1.2 - max_acceleration: 0.8 - kp: 5.0 - tolerance: 0.01 - angular: - max_velocity: 8.0 - max_acceleration: 5.0 - kp: 8.0 - tolerance: 0.01 + diff --git a/mep3_navigation/params/nav2_params_small.yaml b/mep3_navigation/params/nav2_params_small.yaml index 3ccb4e860..6e2f9a7c7 100644 --- a/mep3_navigation/params/nav2_params_small.yaml +++ b/mep3_navigation/params/nav2_params_small.yaml @@ -177,13 +177,11 @@ costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 100.0 - behavior_plugins: ["spin", "wait", "move"] + behavior_plugins: ["spin", "wait"] spin: plugin: "nav2_behaviors/Spin" wait: plugin: "nav2_behaviors/Wait" - move: - plugin: "mep3_navigation::MoveBehavior" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 @@ -192,17 +190,3 @@ min_rotational_vel: 0.4 rotational_acc_lim: 3.2 simulate_ahead_time: 0.3 - - # Move - simulate_ahead_distance: 0.2 - debouncing_duration: 0.05 - linear: - max_velocity: 1.2 - max_acceleration: 1.0 - kp: 5.0 - tolerance: 0.01 - angular: - max_velocity: 8.0 - max_acceleration: 5.0 - kp: 8.0 - tolerance: 0.01 diff --git a/mep3_navigation/src/move_behavior/README.md b/mep3_navigation/src/move/README.md similarity index 100% rename from mep3_navigation/src/move_behavior/README.md rename to mep3_navigation/src/move/README.md diff --git a/mep3_navigation/src/move/move.cpp b/mep3_navigation/src/move/move.cpp new file mode 100644 index 000000000..b023d5e39 --- /dev/null +++ b/mep3_navigation/src/move/move.cpp @@ -0,0 +1,583 @@ +#include "move.hpp" + +#define sign(x) (((x) > 0) - ((x) < 0)) + +namespace mep3_navigation +{ + void Move::on_command_received(const mep3_msgs::msg::MoveCommand::SharedPtr msg) + { + if (state_ == mep3_msgs::msg::MoveState::STATE_IDLE) + { + init_move(msg); + return; + } + + command_->target = msg->target; + command_->header.stamp = msg->header.stamp; + + update_odom_target_tf(); + } + + void Move::on_action() + { + auto result = std::make_shared(); + auto goal = action_server_->get_current_goal(); + + if (state_ != mep3_msgs::msg::MoveState::STATE_IDLE) + { + action_server_->terminate_current(result); + return; + } + + mep3_msgs::msg::MoveCommand::SharedPtr msg = std::make_shared(); + msg->header = goal->header; + msg->odom_frame = goal->odom_frame; + msg->target = goal->target; + msg->linear_properties = goal->linear_properties; + msg->angular_properties = goal->angular_properties; + msg->ignore_obstacles = goal->ignore_obstacles; + msg->timeout = goal->timeout; + msg->reversing = goal->reversing; + msg->mode = goal->mode; + + init_move(msg); + + while (rclcpp::ok() && state_ != mep3_msgs::msg::MoveState::STATE_IDLE) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + result->error = state_msg_.error; + if (state_msg_.error == mep3_msgs::msg::MoveState::ERROR_NONE) + action_server_->succeeded_current(result); + else + action_server_->terminate_current(result); + } + + bool Move::update_odom_target_tf() + { + tf2::Transform tf_global_target; + tf_global_target.setOrigin(tf2::Vector3(command_->target.x, command_->target.y, 0.0)); + tf_global_target.setRotation(tf2::Quaternion( + tf2::Vector3(0, 0, 1), command_->target.theta)); + + geometry_msgs::msg::TransformStamped tf_global_odom_message; + try + { + tf_global_odom_message = tf_->lookupTransform(command_->header.frame_id, command_->odom_frame, command_->header.stamp); + } + catch (const tf2::TransformException &ex) + { + RCLCPP_ERROR(get_logger(), "Initial global_frame -> command_->odom_frame is not available."); + + state_msg_.error = mep3_msgs::msg::MoveState::ERROR_MISSING_TRANSFORM; + state_ = mep3_msgs::msg::MoveState::STATE_IDLE; + state_pub_->publish(state_msg_); + + return false; + } + + tf2::Transform tf_global_odom; + tf2::convert(tf_global_odom_message.transform, tf_global_odom); + tf_odom_target_ = tf_global_odom.inverse() * tf_global_target; + target_updated_ = true; + + // Reset multiturn + multiturn_n_ = 0; + use_multiturn_ = false; + previous_yaw_ = tf2::getYaw(tf_global_target.getRotation()); + return true; + } + + bool Move::init_move(const mep3_msgs::msg::MoveCommand::SharedPtr command) + { + command_ = command; + end_time_ = command_->timeout + now(); + + // Apply defaults + if (command_->header.frame_id == "") + command_->header.frame_id = "map"; + if (command_->odom_frame == "") + command_->odom_frame = "odom"; + if (command_->linear_properties.max_velocity == 0.0) + command_->linear_properties.max_velocity = default_command_->linear_properties.max_velocity; + if (command_->linear_properties.max_acceleration == 0.0) + command_->linear_properties.max_acceleration = default_command_->linear_properties.max_acceleration; + if (command_->linear_properties.kp == 0.0) + command_->linear_properties.kp = default_command_->linear_properties.kp; + if (command_->linear_properties.kd == 0.0) + command_->linear_properties.kd = default_command_->linear_properties.kd; + if (command_->linear_properties.tolerance == 0.0) + command_->linear_properties.tolerance = default_command_->linear_properties.tolerance; + if (command_->angular_properties.max_velocity == 0.0) + command_->angular_properties.max_velocity = default_command_->angular_properties.max_velocity; + if (command_->angular_properties.max_acceleration == 0.0) + command_->angular_properties.max_acceleration = default_command_->angular_properties.max_acceleration; + if (command_->angular_properties.kp == 0.0) + command_->angular_properties.kp = default_command_->angular_properties.kp; + if (command_->angular_properties.kd == 0.0) + command_->angular_properties.kd = default_command_->angular_properties.kd; + if (command_->angular_properties.tolerance == 0.0) + command_->angular_properties.tolerance = default_command_->angular_properties.tolerance; + + if (!update_odom_target_tf()) + return false; + + // Kickoff FSM + lock_tf_odom_base_ = false; + if (command_->mode & mep3_msgs::msg::MoveCommand::MODE_ROTATE_TOWARDS_GOAL) + { + state_ = mep3_msgs::msg::MoveState::STATE_ROTATING_TOWARDS_GOAL; + return true; + } + if (command_->mode & mep3_msgs::msg::MoveCommand::MODE_TRANSLATE) + { + state_ = mep3_msgs::msg::MoveState::STATE_TRANSLATING; + return true; + } + if (command_->mode & mep3_msgs::msg::MoveCommand::MODE_ROTATE_AT_GOAL) + { + state_ = mep3_msgs::msg::MoveState::STATE_ROTATING_AT_GOAL; + + // Multiturn. We allow multiturn only if the goal is in the base frame. + if (command_->mode == mep3_msgs::msg::MoveCommand::MODE_ROTATE_AT_GOAL && command_->header.frame_id == "base_link") + { + if (command->target.theta > M_PI) + multiturn_n_ = (command->target.theta + M_PI) / (2 * M_PI); + else if (command->target.theta < -M_PI) + multiturn_n_ = (command->target.theta - M_PI) / (2 * M_PI); + use_multiturn_ = true; + } + return true; + } + RCLCPP_ERROR(get_logger(), "Invalid MoveCommand, at least one of rotate_towards_goal, translate, or rotate_at_goal must be true."); + return false; + } + + double Move::get_diff_final_orientation(const tf2::Transform &tf_base_target) + { + const double final_yaw_raw = tf2::getYaw(tf_base_target.getRotation()); + if (use_multiturn_) + { + if (final_yaw_raw - previous_yaw_ > M_PI) + multiturn_n_--; + else if (final_yaw_raw - previous_yaw_ < -M_PI) + multiturn_n_++; + } + previous_yaw_ = final_yaw_raw; + return final_yaw_raw + multiturn_n_ * 2 * M_PI; + } + + double Move::get_diff_heading(const tf2::Transform &tf_base_target) + { + if (command_->reversing == mep3_msgs::msg::MoveCommand::REVERSING_AUTO) + { + const double diff_yaw_back = atan2(-tf_base_target.getOrigin().y(), -tf_base_target.getOrigin().x()); + const double diff_yaw_forward = atan2(tf_base_target.getOrigin().y(), tf_base_target.getOrigin().x()); + return (abs(diff_yaw_back) < abs(diff_yaw_forward)) ? diff_yaw_back : diff_yaw_forward; + } + if (command_->reversing == mep3_msgs::msg::MoveCommand::REVERSING_FORCE) + return atan2(-tf_base_target.getOrigin().y(), -tf_base_target.getOrigin().x()); + return atan2(tf_base_target.getOrigin().y(), tf_base_target.getOrigin().x()); + } + + double Move::get_distance(const tf2::Transform &tf_base_target) + { + return sqrt(tf_base_target.getOrigin().x() * tf_base_target.getOrigin().x() + tf_base_target.getOrigin().y() * tf_base_target.getOrigin().y()); + } + + void Move::state_rotating_towards_goal(const tf2::Transform &tf_base_target, const tf2::Transform &tf_odom_base, geometry_msgs::msg::Twist *cmd_vel) + { + const bool should_init = (state_ != previous_state_); + const double diff_yaw = get_diff_heading(tf_base_target); + + if (should_init) + { + debouncing_reset(); + const double distance_to_goal = get_distance(tf_base_target); + if (distance_to_goal < command_->linear_properties.tolerance) + { + // In case we are already at the goal we skip rotation towards the goal and translation. + state_ = mep3_msgs::msg::MoveState::STATE_ROTATING_AT_GOAL; + return; + } + else + { + // TODO: Parametrize this threshold + if (distance_to_goal < 0.15) + { + // When a robot is very close to the goal we cannot use atan2(diff_y, diff_x) as the goal shifts during the rotation. + lock_tf_odom_base_ = true; + locked_tf_odom_base_ = tf_odom_base; + } + init_rotation(diff_yaw); + } + } + + regulate_rotation(cmd_vel, diff_yaw); + if (abs(diff_yaw) < command_->angular_properties.tolerance) + { + if (now() >= debouncing_end_) + { + stop_robot(); + lock_tf_odom_base_ = false; + state_ = mep3_msgs::msg::MoveState::STATE_TRANSLATING; + debouncing_reset(); + } + return; + } + debouncing_reset(); + } + + void Move::stop_robot() + { + geometry_msgs::msg::Twist cmd_vel; + cmd_vel_pub_->publish(cmd_vel); + } + + void Move::state_translating(const tf2::Transform &tf_base_target, geometry_msgs::msg::Twist *cmd_vel) + { + const bool should_init = (state_ != previous_state_); + if (should_init) + { + debouncing_reset(); + init_translation(tf_base_target.getOrigin().x(), tf_base_target.getOrigin().y()); + } + + regulate_translation(cmd_vel, tf_base_target.getOrigin().x(), tf_base_target.getOrigin().y()); + if (abs(tf_base_target.getOrigin().x()) < command_->linear_properties.tolerance) + { + if (now() >= debouncing_end_) + { + stop_robot(); + if (command_->mode & mep3_msgs::msg::MoveCommand::MODE_ROTATE_AT_GOAL) + { + state_ = mep3_msgs::msg::MoveState::STATE_ROTATING_AT_GOAL; + debouncing_reset(); + } + else + { + state_ = mep3_msgs::msg::MoveState::STATE_IDLE; + return; + } + } + return; + } + debouncing_reset(); + } + + void Move::state_rotating_at_goal(const tf2::Transform &tf_base_target, geometry_msgs::msg::Twist *cmd_vel) + { + const bool should_init = (state_ != previous_state_); + const double final_yaw = get_diff_final_orientation(tf_base_target); + + if (should_init) + { + debouncing_reset(); + init_rotation(final_yaw); + } + + regulate_rotation(cmd_vel, final_yaw); + if (abs(final_yaw) < command_->angular_properties.tolerance) + { + if (now() >= debouncing_end_) + { + stop_robot(); + debouncing_reset(); + state_ = mep3_msgs::msg::MoveState::STATE_IDLE; + } + return; + } + debouncing_reset(); + } + + void Move::update() + { + if (state_ == mep3_msgs::msg::MoveState::STATE_IDLE) + { + previous_state_ = state_; + return; + } + + const uint8_t previous_state = state_; + + // Timeout + rclcpp::Duration time_remaining = end_time_ - now(); + if (time_remaining.seconds() < 0.0 && rclcpp::Duration(command_->timeout).seconds() > 0.0) + { + stop_robot(); + RCLCPP_WARN( + get_logger(), + "Exceeded time allowance before reaching the Move goal - Exiting Move"); + state_ = mep3_msgs::msg::MoveState::STATE_IDLE; + return; + } + + // Target in the base frame + geometry_msgs::msg::PoseStamped tf_odom_base_message; + if (!nav2_util::getCurrentPose( + tf_odom_base_message, *tf_, command_->odom_frame, robot_frame_, + transform_tolerance_)) + { + RCLCPP_ERROR(get_logger(), "Initial odom_frame -> base frame is not available."); + state_ = mep3_msgs::msg::MoveState::STATE_IDLE; + return; + } + tf2::Transform tf_odom_base; + tf2::convert(tf_odom_base_message.pose, tf_odom_base); + if (lock_tf_odom_base_) + { + tf_odom_base.getOrigin().setX(locked_tf_odom_base_.getOrigin().x()); + tf_odom_base.getOrigin().setY(locked_tf_odom_base_.getOrigin().y()); + } + tf2::Transform tf_base_target = tf_odom_base.inverse() * tf_odom_target_; + + // FSM + state_msg_.error = mep3_msgs::msg::MoveState::ERROR_NONE; + auto cmd_vel = std::make_unique(); + switch (state_) + { + case mep3_msgs::msg::MoveState::STATE_ROTATING_TOWARDS_GOAL: + state_rotating_towards_goal(tf_base_target, tf_odom_base, cmd_vel.get()); + break; + case mep3_msgs::msg::MoveState::STATE_TRANSLATING: + state_translating(tf_base_target, cmd_vel.get()); + break; + case mep3_msgs::msg::MoveState::STATE_ROTATING_AT_GOAL: + state_rotating_at_goal(tf_base_target, cmd_vel.get()); + break; + } + + // Stop if there is a collision + if (!command_->ignore_obstacles) + { + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, odom_frame_, robot_frame_, + transform_tolerance_)) + { + RCLCPP_ERROR(get_logger(), "Current robot pose is not available."); + state_ = mep3_msgs::msg::MoveState::STATE_IDLE; + return; + } + + geometry_msgs::msg::Pose2D pose2d; + pose2d.x = current_pose.pose.position.x; + pose2d.y = current_pose.pose.position.y; + pose2d.theta = tf2::getYaw(current_pose.pose.orientation); + + const double stopping_distance = stopping_distance_ + (cmd_vel->linear.x * cmd_vel->linear.x) / (2 * command_->linear_properties.max_acceleration); + const double sim_position_change = sign(cmd_vel->linear.x) * stopping_distance; + pose2d.x += sim_position_change * cos(pose2d.theta); + pose2d.y += sim_position_change * sin(pose2d.theta); + + bool is_collision_ahead = false; + try + { + const double score = collision_checker_->scorePose(pose2d); + if (score >= 254) + is_collision_ahead = true; + } + catch (const std::exception &e) + { + RCLCPP_ERROR_ONCE(get_logger(), "Collision checker failed: %s", e.what()); + } + + if (is_collision_ahead) + { + stop_robot(); + RCLCPP_WARN(get_logger(), "Collision Ahead - Exiting Move"); + + state_msg_.error = mep3_msgs::msg::MoveState::ERROR_OBSTACLE; + + state_ = mep3_msgs::msg::MoveState::STATE_IDLE; + update_state_msg(tf_base_target); + state_pub_->publish(state_msg_); + return; + } + } + + cmd_vel_pub_->publish(std::move(cmd_vel)); + + update_state_msg(tf_base_target); + state_pub_->publish(state_msg_); + + previous_state_ = previous_state; + } + + void Move::update_state_msg(tf2::Transform &tf_base_target) + { + state_msg_.state = state_; + state_msg_.distance_xy = get_distance(tf_base_target); + state_msg_.distance_x = tf_base_target.getOrigin().x(); + state_msg_.distance_yaw = get_diff_final_orientation(tf_base_target); + } + + void Move::init_rotation(double diff_yaw) + { + if (rotation_ruckig_ != nullptr) + delete rotation_ruckig_; + + rotation_ruckig_ = new ruckig::Ruckig<1>{1.0 / update_rate_}; + rotation_ruckig_input_.max_velocity = {command_->angular_properties.max_velocity}; + rotation_ruckig_input_.max_acceleration = {command_->angular_properties.max_acceleration}; + rotation_ruckig_input_.max_jerk = {99999999999.0}; + rotation_ruckig_input_.target_position = {0}; + rotation_ruckig_input_.current_position = {diff_yaw}; + rotation_ruckig_input_.control_interface = ruckig::ControlInterface::Position; + rotation_ruckig_->update(rotation_ruckig_input_, rotation_ruckig_output_); + + last_error_yaw_ = 0; + } + + void Move::regulate_rotation(geometry_msgs::msg::Twist *cmd_vel, double diff_yaw) + { + if (target_updated_) + { + double prev = rotation_ruckig_input_.current_position[0]; + rotation_ruckig_input_.current_position[0] = diff_yaw - last_error_yaw_; + target_updated_ = false; + } + + const double previous_input = rotation_ruckig_output_.new_position[0]; + const bool is_trajectory_finished = (rotation_ruckig_->update(rotation_ruckig_input_, rotation_ruckig_output_) == ruckig::Finished); + last_error_yaw_ = diff_yaw - rotation_ruckig_output_.new_position[0]; + const double d_input = rotation_ruckig_output_.new_position[0] - previous_input; + cmd_vel->angular.z = command_->angular_properties.kp * last_error_yaw_ - command_->angular_properties.kd * d_input; + + if (!is_trajectory_finished) + rotation_ruckig_output_.pass_to_input(rotation_ruckig_input_); + } + + void Move::init_translation(double diff_x, double diff_y) + { + if (translation_ruckig_ != nullptr) + delete translation_ruckig_; + + translation_ruckig_ = new ruckig::Ruckig<1>{1.0 / update_rate_}; + translation_ruckig_input_.max_velocity = {command_->linear_properties.max_velocity}; + translation_ruckig_input_.max_acceleration = {command_->linear_properties.max_acceleration}; + translation_ruckig_input_.max_jerk = {99999999999.0}; + translation_ruckig_input_.target_position = {0}; + translation_ruckig_input_.current_position = {diff_x}; + translation_ruckig_input_.control_interface = ruckig::ControlInterface::Position; + translation_ruckig_->update(translation_ruckig_input_, translation_ruckig_output_); + + last_error_x_ = 0; + last_error_y_ = 0; + } + + void Move::regulate_translation(geometry_msgs::msg::Twist *cmd_vel, double diff_x, double diff_y) + { + if (target_updated_) + { + double prev = translation_ruckig_input_.current_position[0]; + translation_ruckig_input_.current_position[0] = diff_x - last_error_x_; + RCLCPP_INFO(get_logger(), "diff_x: %f, last_error_x_: %f", diff_x, last_error_x_); + RCLCPP_INFO(get_logger(), "prev: %f, current: %f", prev, translation_ruckig_input_.current_position[0]); + target_updated_ = false; + } + + const double previous_input = translation_ruckig_output_.new_position[0]; + const bool is_trajectory_finished = (translation_ruckig_->update(translation_ruckig_input_, translation_ruckig_output_) == ruckig::Finished); + last_error_x_ = diff_x - translation_ruckig_output_.new_position[0]; + const double d_input = translation_ruckig_output_.new_position[0] - previous_input; + cmd_vel->linear.x = command_->linear_properties.kp * last_error_x_ - command_->linear_properties.kd * d_input; + + // TODO: Parameterize this + add kd + cmd_vel->angular.z = diff_y * cmd_vel->linear.x * 1.0; + + if (!is_trajectory_finished) + translation_ruckig_output_.pass_to_input(translation_ruckig_input_); + } + + void Move::init() + { + cmd_vel_pub_ = create_publisher("cmd_vel", 1); + command_sub_ = create_subscription( + "~/command", 1, std::bind(&Move::on_command_received, this, std::placeholders::_1)); + state_pub_ = create_publisher("~/state", 1); + action_server_ = std::make_shared>( + this, "~/move", std::bind(&Move::on_action, this)); + action_server_->activate(); + + tf_ = + std::make_unique(get_clock()); + tf_listener_ = + std::make_shared(*tf_); + + costmap_sub_ = std::make_shared( + shared_from_this(), "local_costmap/costmap_raw"); + footprint_sub_ = std::make_shared( + shared_from_this(), "local_costmap/published_footprint", *tf_, robot_frame_, transform_tolerance_); + collision_checker_ = + std::make_shared( + *costmap_sub_, *footprint_sub_, get_name()); + } + + Move::Move(std::string name) : Node(name) + { + // Read parameters + declare_parameter("update_rate", rclcpp::ParameterValue(50)); + get_parameter("update_rate", update_rate_); + + double command_timeout; + declare_parameter("command_timeout", rclcpp::ParameterValue(0.5)); + get_parameter("command_timeout", command_timeout); + command_timeout_ = rclcpp::Duration::from_seconds(command_timeout); + + double debouncing_duration; + declare_parameter("debouncing_duration", rclcpp::ParameterValue(0.3)); + get_parameter("debouncing_duration", debouncing_duration); + debouncing_duration_ = rclcpp::Duration::from_seconds(debouncing_duration); + + declare_parameter("stopping_distance", rclcpp::ParameterValue(0.2)); + get_parameter("stopping_distance", stopping_distance_); + + declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.5)); + get_parameter("transform_tolerance", transform_tolerance_); + + declare_parameter("robot_frame", rclcpp::ParameterValue(std::string("base_link"))); + get_parameter("robot_frame", robot_frame_); + + declare_parameter("odom_frame", rclcpp::ParameterValue(std::string("odom"))); + get_parameter("odom_frame", odom_frame_); + + // Linear + declare_parameter("linear.kp", rclcpp::ParameterValue(3.0)); + get_parameter("linear.kp", default_command_->linear_properties.kp); + + declare_parameter("linear.kd", rclcpp::ParameterValue(0.0)); + get_parameter("linear.kd", default_command_->linear_properties.kd); + + declare_parameter("linear.max_velocity", rclcpp::ParameterValue(0.1)); + get_parameter("linear.max_velocity", default_command_->linear_properties.max_velocity); + + declare_parameter("linear.max_acceleration", rclcpp::ParameterValue(1.5)); + get_parameter("linear.max_acceleration", default_command_->linear_properties.max_acceleration); + + declare_parameter("linear.tolerance", rclcpp::ParameterValue(0.01)); + get_parameter("linear.tolerance", default_command_->linear_properties.tolerance); + + // Angular + declare_parameter("angular.kp", rclcpp::ParameterValue(5.0)); + get_parameter("angular.kp", default_command_->angular_properties.kp); + + declare_parameter("angular.kd", rclcpp::ParameterValue(0.0)); + get_parameter("angular.kd", default_command_->angular_properties.kd); + + declare_parameter("angular.max_velocity", rclcpp::ParameterValue(0.5)); + get_parameter("angular.max_velocity", default_command_->angular_properties.max_velocity); + + declare_parameter("angular.max_acceleration", rclcpp::ParameterValue(0.1)); + get_parameter("angular.max_acceleration", default_command_->angular_properties.max_acceleration); + + declare_parameter("angular.tolerance", rclcpp::ParameterValue(0.03)); + get_parameter("angular.tolerance", default_command_->angular_properties.tolerance); + } + + void Move::debouncing_reset() + { + debouncing_end_ = now() + debouncing_duration_; + } +}; diff --git a/mep3_navigation/src/move/move.hpp b/mep3_navigation/src/move/move.hpp new file mode 100644 index 000000000..45372506e --- /dev/null +++ b/mep3_navigation/src/move/move.hpp @@ -0,0 +1,111 @@ +#ifndef MEP3_NAVIGATION__MOVE_HPP_ +#define MEP3_NAVIGATION__MOVE_HPP_ + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" +#include "nav2_costmap_2d/footprint_subscriber.hpp" +#include "nav2_util/execution_timer.hpp" +#include "nav2_util/simple_action_server.hpp" +#include "mep3_msgs/msg/move_command.hpp" +#include "mep3_msgs/msg/move_properties.hpp" +#include "mep3_msgs/msg/move_state.hpp" +#include "mep3_msgs/action/move.hpp" +#include "ruckig/ruckig.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace mep3_navigation +{ + class Move : public rclcpp::Node + { + public: + Move(std::string name); + void init(); + void update(); + int get_update_rate() { return update_rate_; }; + + private: + void on_action(); + void on_command_received(const mep3_msgs::msg::MoveCommand::SharedPtr msg); + + bool init_move(const mep3_msgs::msg::MoveCommand::SharedPtr msg); + void init_rotation(double diff_yaw); + void regulate_rotation(geometry_msgs::msg::Twist *cmd_vel, double diff_yaw); + void init_translation(double diff_x, double diff_y); + void regulate_translation(geometry_msgs::msg::Twist *cmd_vel, double diff_x, double diff_y); + + void state_rotating_towards_goal(const tf2::Transform &tf_base_target, const tf2::Transform &tf_odom_base, geometry_msgs::msg::Twist *cmd_vel); + void state_translating(const tf2::Transform &tf_base_target, geometry_msgs::msg::Twist *cmd_vel); + void state_rotating_at_goal(const tf2::Transform &tf_base_target, geometry_msgs::msg::Twist *cmd_vel); + void state_stopping(); + + double get_diff_heading(const tf2::Transform &tf_base_target); + double get_diff_final_orientation(const tf2::Transform &tf_base_target); + double get_distance(const tf2::Transform &tf_base_target); + + bool update_odom_target_tf(); + + void stop_robot(); + void update_state_msg(tf2::Transform &tf_base_target); + + int update_rate_; + double stopping_distance_; + + mep3_msgs::msg::MoveState state_msg_; + + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Subscription::SharedPtr command_sub_; + rclcpp::Publisher::SharedPtr state_pub_; + std::shared_ptr> action_server_; + + std::shared_ptr costmap_sub_; + std::shared_ptr footprint_sub_; + std::shared_ptr collision_checker_; + + std::shared_ptr tf_listener_{nullptr}; + std::unique_ptr tf_; + + mep3_msgs::msg::MoveCommand::SharedPtr command_; + mep3_msgs::msg::MoveCommand::SharedPtr default_command_{new mep3_msgs::msg::MoveCommand()}; + + std::string robot_frame_; + std::string odom_frame_; + + tf2::Transform tf_odom_target_; + rclcpp::Time end_time_; + bool target_updated_{false}; + double last_error_x_; + double last_error_y_; + double last_error_yaw_; + + rclcpp::Time debouncing_end_; + rclcpp::Duration debouncing_duration_{0, 0}; + void debouncing_reset(); + + ruckig::Ruckig<1> *rotation_ruckig_{nullptr}; + ruckig::InputParameter<1> rotation_ruckig_input_; + ruckig::OutputParameter<1> rotation_ruckig_output_; + double previous_yaw_; + int multiturn_n_; + bool use_multiturn_; + + tf2::Transform locked_tf_odom_base_; + bool lock_tf_odom_base_{false}; + + ruckig::Ruckig<1> *translation_ruckig_{nullptr}; + ruckig::InputParameter<1> translation_ruckig_input_; + ruckig::OutputParameter<1> translation_ruckig_output_; + + rclcpp::Duration command_timeout_{0, 0}; + rclcpp::Time last_command_received_; + + uint8_t state_{mep3_msgs::msg::MoveState::STATE_IDLE}; + uint8_t previous_state_{mep3_msgs::msg::MoveState::STATE_IDLE}; + + // TODO: Remove + double transform_tolerance_{0.5}; + }; + +} + +#endif // MEP3_NAVIGATION__MOVE_HPP_ diff --git a/mep3_navigation/src/move/move_node.cpp b/mep3_navigation/src/move/move_node.cpp new file mode 100644 index 000000000..e6cdd30bb --- /dev/null +++ b/mep3_navigation/src/move/move_node.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include + +#include "move.hpp" +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + std::shared_ptr executor = + std::make_shared(); + auto move = std::make_shared("move"); + move->init(); + + RCLCPP_INFO(move->get_logger(), "update rate is %d Hz", move->get_update_rate()); + + // As here: https://github.com/ros-controls/ros2_control/blob/master/controller_manager/src/ros2_control_node.cpp + std::thread move_thread( + [move]() + { + rclcpp::Time end_period = move->now(); + rclcpp::Duration period(std::chrono::nanoseconds(1000000000 / move->get_update_rate())); + + while (rclcpp::ok()) + { + end_period += period; + std::this_thread::sleep_for(std::chrono::nanoseconds((end_period - move->now()).nanoseconds())); + + move->update(); + } + }); + + executor->add_node(move); + executor->spin(); + move_thread.join(); + rclcpp::shutdown(); + return 0; +} diff --git a/mep3_navigation/src/move_behavior/move_behavior.cpp b/mep3_navigation/src/move_behavior/move_behavior.cpp deleted file mode 100644 index 32f58d329..000000000 --- a/mep3_navigation/src/move_behavior/move_behavior.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include -#include "move_behavior.hpp" - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(mep3_navigation::MoveBehavior, nav2_core::Behavior) diff --git a/mep3_navigation/src/move_behavior/move_behavior.hpp b/mep3_navigation/src/move_behavior/move_behavior.hpp deleted file mode 100644 index 7482cb152..000000000 --- a/mep3_navigation/src/move_behavior/move_behavior.hpp +++ /dev/null @@ -1,494 +0,0 @@ -#ifndef MEP3_NAVIGATION__MOVE_BEHAVIOR_HPP_ -#define MEP3_NAVIGATION__MOVE_BEHAVIOR_HPP_ - -#include -#include -#include - -#include "nav2_behaviors/timed_behavior.hpp" -#include "mep3_msgs/action/move.hpp" -#include "mep3_msgs/msg/motion_properties.hpp" -#include "nav2_util/node_utils.hpp" -#include "ruckig/ruckig.hpp" -#include "mep3_navigation/stuck_detector.hpp" - -#define sign(x) (((x) > 0) - ((x) < 0)) -#define min(x, y) (((x) < (y)) ? (x) : (y)) - -namespace mep3_navigation -{ - enum MoveState - { - INITIALIZE_ROTATION_TOWARDS_GOAL, - REGULATE_ROTATION_TOWARDS_GOAL, - INITIALIZE_TRANSLATION, - REGULATE_TRANSLATION, - INITIALIZE_ROTATION_AT_GOAL, - REGULATE_ROTATION_AT_GOAL - }; - - using ActionT = mep3_msgs::action::Move; - - class MoveBehavior : public nav2_behaviors::TimedBehavior - { - public: - MoveBehavior() - : nav2_behaviors::TimedBehavior(), - feedback_(std::make_shared()) - { - } - - ~MoveBehavior() = default; - - nav2_behaviors::Status onRun(const std::shared_ptr command) override - { - command_global_frame_ = command->header.frame_id; - odom_frame_ = command->odom_frame; - ignore_obstacles_ = command->ignore_obstacles; - timeout_ = command->timeout; - end_time_ = steady_clock_.now() + timeout_; - linear_properties_ = command->linear_properties; - angular_properties_ = command->angular_properties; - type_ = command->type; - - // Apply defaults - if (command_global_frame_ == "") - command_global_frame_ = "map"; - if (odom_frame_ == "") - odom_frame_ = "odom"; - if (linear_properties_.max_velocity == 0.0) - linear_properties_.max_velocity = default_linear_properties_.max_velocity; - if (linear_properties_.max_acceleration == 0.0) - linear_properties_.max_acceleration = default_linear_properties_.max_acceleration; - if (linear_properties_.kp == 0.0) - linear_properties_.kp = default_linear_properties_.kp; - if (linear_properties_.kd == 0.0) - linear_properties_.kd = default_linear_properties_.kd; - if (linear_properties_.tolerance == 0.0) - linear_properties_.tolerance = default_linear_properties_.tolerance; - if (angular_properties_.max_velocity == 0.0) - angular_properties_.max_velocity = default_angular_properties_.max_velocity; - if (angular_properties_.max_acceleration == 0.0) - angular_properties_.max_acceleration = default_angular_properties_.max_acceleration; - if (angular_properties_.kp == 0.0) - angular_properties_.kp = default_angular_properties_.kp; - if (angular_properties_.kd == 0.0) - angular_properties_.kd = default_angular_properties_.kd; - if (angular_properties_.tolerance == 0.0) - angular_properties_.tolerance = default_angular_properties_.tolerance; - - // Target in the global frame - tf2::Transform tf_global_target; - tf_global_target.setOrigin(tf2::Vector3(command->target.x, command->target.y, 0.0)); - tf_global_target.setRotation(tf2::Quaternion( - tf2::Vector3(0, 0, 1), command->target.theta)); - - geometry_msgs::msg::PoseStamped tf_global_odom_message; - if (!nav2_util::getCurrentPose( - tf_global_odom_message, *this->tf_, command_global_frame_, odom_frame_, - this->transform_tolerance_)) - { - RCLCPP_ERROR(this->logger_, "Initial global_frame -> odom_frame_ is not available."); - return nav2_behaviors::Status::FAILED; - } - tf2::Transform tf_global_odom; - tf2::convert(tf_global_odom_message.pose, tf_global_odom); - tf_odom_target_ = tf_global_odom.inverse() * tf_global_target; - - // Reset multiturn - multiturn_n_ = 0; - use_multiturn_ = false; - previous_yaw_ = tf2::getYaw(tf_global_target.getRotation()); - - // Kickoff FSM - lock_tf_odom_base_ = false; - switch (type_) - { - case mep3_msgs::action::Move::Goal::TYPE_ROTATE: - state_ = MoveState::INITIALIZE_ROTATION_AT_GOAL; - - // Multiturn support - if (command_global_frame_ == "base_link") - { - if (command->target.theta > M_PI) - multiturn_n_ = (command->target.theta + M_PI) / (2 * M_PI); - else if (command->target.theta < -M_PI) - multiturn_n_ = (command->target.theta - M_PI) / (2 * M_PI); - use_multiturn_ = true; - } - break; - case mep3_msgs::action::Move::Goal::TYPE_TRANSLATE: - state_ = MoveState::INITIALIZE_TRANSLATION; - break; - case mep3_msgs::action::Move::Goal::TYPE_FULL_NO_REVERSING: - case mep3_msgs::action::Move::Goal::TYPE_FULL_AUTO_REVERSING: - case mep3_msgs::action::Move::Goal::TYPE_FULL_FORCE_REVERSING: - case mep3_msgs::action::Move::Goal::TYPE_SKIP_FINAL_ROTATION: - state_ = MoveState::INITIALIZE_ROTATION_TOWARDS_GOAL; - break; - } - return nav2_behaviors::Status::SUCCEEDED; - } - - nav2_behaviors::Status onCycleUpdate() - { - // Timeout - rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); - if (time_remaining.seconds() < 0.0 && timeout_.seconds() > 0.0) - { - stopRobot(); - RCLCPP_WARN( - this->logger_, - "Exceeded time allowance before reaching the Move goal - Exiting Move"); - return nav2_behaviors::Status::FAILED; - } - - // Target in the base frame - geometry_msgs::msg::PoseStamped tf_odom_base_message; - if (!nav2_util::getCurrentPose( - tf_odom_base_message, *this->tf_, odom_frame_, robot_base_frame_, - this->transform_tolerance_)) - { - RCLCPP_ERROR(this->logger_, "Initial odom_frame -> base frame is not available."); - return nav2_behaviors::Status::FAILED; - } - tf2::Transform tf_odom_base; - tf2::convert(tf_odom_base_message.pose, tf_odom_base); - if (lock_tf_odom_base_) - { - tf_odom_base.getOrigin().setX(locked_tf_odom_base_.getOrigin().x()); - tf_odom_base.getOrigin().setY(locked_tf_odom_base_.getOrigin().y()); - } - const tf2::Transform tf_base_target = tf_odom_base.inverse() * tf_odom_target_; - - const double final_yaw_raw = tf2::getYaw(tf_base_target.getRotation()); - if (use_multiturn_) { - if (final_yaw_raw - previous_yaw_ > M_PI) - multiturn_n_--; - else if (final_yaw_raw - previous_yaw_ < -M_PI) - multiturn_n_++; - } - previous_yaw_ = final_yaw_raw; - const double final_yaw = final_yaw_raw + multiturn_n_ * 2 * M_PI; - const double diff_x = tf_base_target.getOrigin().x(); - const double diff_y = tf_base_target.getOrigin().y(); - - double diff_yaw = 0; - if (type_ == mep3_msgs::action::Move::Goal::TYPE_FULL_AUTO_REVERSING) - { - const double diff_yaw_back = atan2(-diff_y, -diff_x); - const double diff_yaw_forward = atan2(diff_y, diff_x); - diff_yaw = (abs(diff_yaw_back) < abs(diff_yaw_forward)) ? diff_yaw_back : diff_yaw_forward; - } - else if (type_ == mep3_msgs::action::Move::Goal::TYPE_FULL_FORCE_REVERSING) - { - diff_yaw = atan2(-diff_y, -diff_x); - } - else - { - diff_yaw = atan2(diff_y, diff_x); - } - - // FSM - auto cmd_vel = std::make_unique(); - switch (state_) - { - case MoveState::INITIALIZE_ROTATION_TOWARDS_GOAL: - { - const double dinstace_to_goal = sqrt(diff_x * diff_x + diff_y * diff_y); - if (dinstace_to_goal < linear_properties_.tolerance) - { - // In case we are already at the goal we skip rotation towards the goal and translation. - state_ = MoveState::INITIALIZE_ROTATION_AT_GOAL; - } - else - { - if (dinstace_to_goal < 0.15) { - // When a robot is very close to the goal we cannot use atan2(diff_y, diff_x) as the goal shits during the rotation. - lock_tf_odom_base_ = true; - locked_tf_odom_base_ = tf_odom_base; - } - initializeRotation(diff_yaw); - regulateRotation(cmd_vel.get(), diff_yaw); - state_ = MoveState::REGULATE_ROTATION_TOWARDS_GOAL; - } - } - break; - case MoveState::REGULATE_ROTATION_TOWARDS_GOAL: - regulateRotation(cmd_vel.get(), diff_yaw); - if (abs(diff_yaw) < angular_properties_.tolerance) - { - debouncing_counter_++; - if (debouncing_counter_ >= debouncing_counter_max_) - { - stopRobot(); - lock_tf_odom_base_ = false; - state_ = MoveState::INITIALIZE_TRANSLATION; - debouncing_counter_ = 0; - } - } - else - { - debouncing_counter_ = 0; - } - break; - case MoveState::INITIALIZE_TRANSLATION: - initializeTranslation(diff_x, diff_y); - state_ = MoveState::REGULATE_TRANSLATION; - break; - case MoveState::REGULATE_TRANSLATION: - regulateTranslation(cmd_vel.get(), diff_x, diff_y); - if (abs(diff_x) < linear_properties_.tolerance) - { - debouncing_counter_++; - if (debouncing_counter_ >= debouncing_counter_max_) - { - stopRobot(); - if (type_ == mep3_msgs::action::Move::Goal::TYPE_SKIP_FINAL_ROTATION || - type_ == mep3_msgs::action::Move::Goal::TYPE_TRANSLATE) - return nav2_behaviors::Status::SUCCEEDED; - state_ = MoveState::INITIALIZE_ROTATION_AT_GOAL; - debouncing_counter_ = 0; - } - } - else - { - debouncing_counter_ = 0; - } - break; - case MoveState::INITIALIZE_ROTATION_AT_GOAL: - initializeRotation(final_yaw); - regulateRotation(cmd_vel.get(), final_yaw); - state_ = MoveState::REGULATE_ROTATION_AT_GOAL; - break; - case MoveState::REGULATE_ROTATION_AT_GOAL: - regulateRotation(cmd_vel.get(), final_yaw); - if (abs(final_yaw) < angular_properties_.tolerance) - { - debouncing_counter_++; - if (debouncing_counter_ >= debouncing_counter_max_) - { - stopRobot(); - return nav2_behaviors::Status::SUCCEEDED; - debouncing_counter_ = 0; - } - } - else - { - debouncing_counter_ = 0; - } - break; - } - - // Stop if there is a collision - if (!ignore_obstacles_) - { - geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose( - current_pose, *this->tf_, this->global_frame_, this->robot_base_frame_, - this->transform_tolerance_)) - { - RCLCPP_ERROR(this->logger_, "Current robot pose is not available."); - return nav2_behaviors::Status::FAILED; - } - - geometry_msgs::msg::Pose2D pose2d; - pose2d.x = current_pose.pose.position.x; - pose2d.y = current_pose.pose.position.y; - pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - - const double sim_position_change = sign(cmd_vel->linear.x) * simulate_ahead_distance_; - pose2d.x += sim_position_change * cos(pose2d.theta); - pose2d.y += sim_position_change * sin(pose2d.theta); - if (!collision_checker_->isCollisionFree(pose2d)) - { - stopRobot(); - RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting MoveBehavior"); - return nav2_behaviors::Status::FAILED; - } - } - - // Stop if the robot is stuck - if (stuck_detector_->is_stuck()) - { - stopRobot(); - stuck_detector_->softstop(); - RCLCPP_WARN(this->logger_, "Robot is stuck - Exiting MoveBehavior"); - return nav2_behaviors::Status::FAILED; - } - - this->vel_pub_->publish(std::move(cmd_vel)); - return nav2_behaviors::Status::RUNNING; - } - - protected: - void initializeRotation(double diff_yaw) - { - if (rotation_ruckig_ != nullptr) - delete rotation_ruckig_; - - rotation_ruckig_ = new ruckig::Ruckig<1>{1.0 / cycle_frequency_}; - rotation_ruckig_input_.max_velocity = {angular_properties_.max_velocity}; - rotation_ruckig_input_.max_acceleration = {angular_properties_.max_acceleration}; - rotation_ruckig_input_.max_jerk = {99999999999.0}; - rotation_ruckig_input_.target_position = {0}; - rotation_ruckig_input_.current_position = {diff_yaw}; - rotation_ruckig_input_.control_interface = ruckig::ControlInterface::Position; - rotation_ruckig_output_.new_position = {diff_yaw}; - rotation_ruckig_output_.new_velocity = {0.0}; - rotation_ruckig_output_.new_acceleration = {0.0}; - rotation_ruckig_output_.pass_to_input(rotation_ruckig_input_); - rotation_last_input_ = rotation_ruckig_output_.new_position[0]; - } - - void regulateRotation(geometry_msgs::msg::Twist *cmd_vel, double diff_yaw) - { - if (rotation_ruckig_->update(rotation_ruckig_input_, rotation_ruckig_output_) != ruckig::Finished) - rotation_ruckig_output_.pass_to_input(rotation_ruckig_input_); - const double error_yaw = diff_yaw - rotation_ruckig_output_.new_position[0]; - const double d_input = rotation_ruckig_output_.new_position[0] - rotation_last_input_; - rotation_last_input_ = rotation_ruckig_output_.new_position[0]; - cmd_vel->angular.z = angular_properties_.kp * error_yaw - angular_properties_.kd * d_input; - } - - void initializeTranslation(double diff_x, double diff_y) - { - if (translation_ruckig_ != nullptr) - delete translation_ruckig_; - - translation_ruckig_ = new ruckig::Ruckig<1>{1.0 / cycle_frequency_}; - translation_ruckig_input_.max_velocity = {linear_properties_.max_velocity}; - translation_ruckig_input_.max_acceleration = {linear_properties_.max_acceleration}; - translation_ruckig_input_.max_jerk = {99999999999.0}; - translation_ruckig_input_.target_position = {0}; - translation_ruckig_input_.current_position = {diff_x}; - translation_ruckig_input_.control_interface = ruckig::ControlInterface::Position; - translation_ruckig_output_.new_position = {diff_x}; - translation_ruckig_output_.new_velocity = {0.0}; - translation_ruckig_output_.new_acceleration = {0.0}; - translation_ruckig_output_.pass_to_input(translation_ruckig_input_); - translation_last_input_ = translation_ruckig_output_.new_position[0]; - } - - void regulateTranslation(geometry_msgs::msg::Twist *cmd_vel, double diff_x, double diff_y) - { - if (translation_ruckig_->update(translation_ruckig_input_, translation_ruckig_output_) != ruckig::Finished) - translation_ruckig_output_.pass_to_input(translation_ruckig_input_); - const double error_x = diff_x - translation_ruckig_output_.new_position[0]; - const double d_input = translation_ruckig_output_.new_position[0] - translation_last_input_; - translation_last_input_ = translation_ruckig_output_.new_position[0]; - cmd_vel->linear.x = linear_properties_.kp * error_x - linear_properties_.kd * d_input; - cmd_vel->angular.z = diff_y * cmd_vel->linear.x * 1.0; - } - - void onConfigure() override - { - auto node = this->node_.lock(); - if (!node) - { - throw std::runtime_error{"Failed to lock node"}; - } - - nav2_util::declare_parameter_if_not_declared( - node, - "simulate_ahead_distance", rclcpp::ParameterValue(0.2)); - node->get_parameter("simulate_ahead_distance", simulate_ahead_distance_); - - double debouncing_duration; - nav2_util::declare_parameter_if_not_declared( - node, - "debouncing_duration", rclcpp::ParameterValue(0.05)); - node->get_parameter("debouncing_duration", debouncing_duration); - debouncing_counter_max_ = static_cast(debouncing_duration * cycle_frequency_); - - // Linear - nav2_util::declare_parameter_if_not_declared( - node, - "linear.kp", rclcpp::ParameterValue(15.0)); - node->get_parameter("linear.kp", default_linear_properties_.kp); - nav2_util::declare_parameter_if_not_declared( - node, - "linear.kd", rclcpp::ParameterValue(0.0)); - node->get_parameter("linear.kd", default_linear_properties_.kd); - nav2_util::declare_parameter_if_not_declared( - node, - "linear.max_velocity", rclcpp::ParameterValue(0.5)); - node->get_parameter("linear.max_velocity", default_linear_properties_.max_velocity); - nav2_util::declare_parameter_if_not_declared( - node, - "linear.max_acceleration", rclcpp::ParameterValue(0.5)); - node->get_parameter("linear.max_acceleration", default_linear_properties_.max_acceleration); - nav2_util::declare_parameter_if_not_declared( - node, - "linear.tolerance", rclcpp::ParameterValue(0.01)); - node->get_parameter("linear.tolerance", default_linear_properties_.tolerance); - - // Angular - nav2_util::declare_parameter_if_not_declared( - node, - "angular.kp", rclcpp::ParameterValue(15.0)); - node->get_parameter("angular.kp", default_angular_properties_.kp); - nav2_util::declare_parameter_if_not_declared( - node, - "angular.kd", rclcpp::ParameterValue(0.0)); - nav2_util::declare_parameter_if_not_declared( - node, - "angular.max_velocity", rclcpp::ParameterValue(0.5)); - node->get_parameter("angular.max_velocity", default_angular_properties_.max_velocity); - nav2_util::declare_parameter_if_not_declared( - node, - "angular.max_acceleration", rclcpp::ParameterValue(0.5)); - node->get_parameter("angular.max_acceleration", default_angular_properties_.max_acceleration); - nav2_util::declare_parameter_if_not_declared( - node, - "angular.tolerance", rclcpp::ParameterValue(0.03)); - node->get_parameter("angular.tolerance", default_angular_properties_.tolerance); - - stuck_detector_ = std::make_shared(node); - } - - typename ActionT::Feedback::SharedPtr feedback_; - - std::string command_global_frame_; - std::string odom_frame_; - tf2::Transform tf_odom_target_; - bool ignore_obstacles_; - - mep3_msgs::msg::MotionProperties linear_properties_; - mep3_msgs::msg::MotionProperties angular_properties_; - - mep3_msgs::msg::MotionProperties default_linear_properties_; - mep3_msgs::msg::MotionProperties default_angular_properties_; - - rclcpp::Duration timeout_{0, 0}; - rclcpp::Time end_time_; - double simulate_ahead_distance_; - - int debouncing_counter_; - int debouncing_counter_max_; - - ruckig::Ruckig<1> *rotation_ruckig_{nullptr}; - ruckig::InputParameter<1> rotation_ruckig_input_; - ruckig::OutputParameter<1> rotation_ruckig_output_; - double rotation_last_input_; - double previous_yaw_; - int multiturn_n_; - bool use_multiturn_; - - tf2::Transform locked_tf_odom_base_; - bool lock_tf_odom_base_{false}; - - ruckig::Ruckig<1> *translation_ruckig_{nullptr}; - ruckig::InputParameter<1> translation_ruckig_input_; - ruckig::OutputParameter<1> translation_ruckig_output_; - double translation_last_input_; - - MoveState state_; - uint8_t type_; - - std::shared_ptr stuck_detector_; - }; - -} // namespace mep3_navigation - -#endif // MEP3_NAVIGATION__MOVE_BEHAVIOR_HPP_ diff --git a/mep3_simulation/launch/simulation_launch.py b/mep3_simulation/launch/simulation_launch.py index af75412e8..76129a8de 100644 --- a/mep3_simulation/launch/simulation_launch.py +++ b/mep3_simulation/launch/simulation_launch.py @@ -9,9 +9,6 @@ def generate_launch_description(): - # HOTFIX: https://github.com/cyberbotics/webots_ros2/issues/567 - os.environ['LD_LIBRARY_PATH'] += ':/opt/ros/humble/lib/controller' - package_dir = get_package_share_directory('mep3_simulation') controller_params_file_big = os.path.join(get_package_share_directory('mep3_hardware'), diff --git a/mep3_simulation/resource/small_description.urdf b/mep3_simulation/resource/small_description.urdf index c1ec249a9..b6db3c4cf 100644 --- a/mep3_simulation/resource/small_description.urdf +++ b/mep3_simulation/resource/small_description.urdf @@ -32,4 +32,86 @@ right_encoder + + + + mep3_simulation::Mep3WebotsHardwareInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mep3_simulation/webots_data/controllers/judge/judge.py b/mep3_simulation/webots_data/controllers/judge/judge.py index 6da2cd1a3..283ad1930 100644 --- a/mep3_simulation/webots_data/controllers/judge/judge.py +++ b/mep3_simulation/webots_data/controllers/judge/judge.py @@ -6,10 +6,10 @@ INITIAL_POSE_MATRIX = [ - ('big', 'blue', [-0.65, -0.43, 0]), - ('small', 'blue', [-0.21, -1.16, pi/2]), - ('big', 'green', [-0.65, 0.43, 0]), - ('small', 'green', [0.21, -1.16, pi/2]), + ('big', 'blue', [-0.72, -1.16, pi/2]), + ('small', 'blue', [0.72, -1.16, pi/2]), + ('big', 'green', [-0.72, 1.16, -pi/2]), + ('small', 'green', [0.72, 1.16, -pi/2]), ] diff --git a/mep3_simulation/webots_data/protos/StaticObjects.proto b/mep3_simulation/webots_data/protos/StaticObjects.proto index 359b22329..eb2dac3af 100644 --- a/mep3_simulation/webots_data/protos/StaticObjects.proto +++ b/mep3_simulation/webots_data/protos/StaticObjects.proto @@ -4,7 +4,6 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/floors/protos/Floor.proto" EXTERNPROTO "Walls.proto" EXTERNPROTO "FixedBeaconSupports.proto" -EXTERNPROTO "CherrySupports.proto" EXTERNPROTO "BasketSupport.proto" EXTERNPROTO "CentralTrackingDevice.proto" @@ -37,7 +36,6 @@ Group { BasketSupport { teamColor "green" } - CherrySupports{} ] } } diff --git a/mep3_simulation/webots_data/worlds/.eurobot.wbproj b/mep3_simulation/webots_data/worlds/.eurobot.wbproj index 1fa3ec672..f82f604c1 100644 --- a/mep3_simulation/webots_data/worlds/.eurobot.wbproj +++ b/mep3_simulation/webots_data/worlds/.eurobot.wbproj @@ -1,10 +1,10 @@ Webots Project File version R2023b -perspectives: 000000ff00000000fd00000002000000010000011c00000307fc0200000001fb0000001400540065007800740045006400690074006f00720000000013000003070000003f00ffffff000000030000031e00000039fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000031e0000006900ffffff0000031e0000030500000001000000020000000100000008fc00000000 +perspectives: 000000ff00000000fd00000002000000010000011c00000307fc0200000001fb0000001400540065007800740045006400690074006f00720000000013000003070000003f00ffffff000000030000078000000039fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000043300000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff000000010000000200000101000002c40100000002010000000100 -sceneTreePerspectives: 000000ff00000001000000030000001c000002ca000000fa0100000002010000000200 +sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000fa0100000002010000000200 maximizedDockId: -1 centralWidgetVisible: 1 orthographicViewHeight: 1 textFiles: -1 consoles: Console:All:All -renderingDevicePerspectives: camera_central:RasPi0;0;0.25463;0;0 +renderingDevicePerspectives: camera_central:RasPi0;0;0.113021;0;0 diff --git a/mep3_simulation/webots_data/worlds/assets/table.png b/mep3_simulation/webots_data/worlds/assets/table.png index 12fdb11ab..656b76cec 100644 Binary files a/mep3_simulation/webots_data/worlds/assets/table.png and b/mep3_simulation/webots_data/worlds/assets/table.png differ diff --git a/mep3_simulation/webots_data/worlds/eurobot.wbt b/mep3_simulation/webots_data/worlds/eurobot.wbt index 663bae11a..aa96ef906 100644 --- a/mep3_simulation/webots_data/worlds/eurobot.wbt +++ b/mep3_simulation/webots_data/worlds/eurobot.wbt @@ -1,7 +1,5 @@ #VRML_SIM R2023b utf8 -EXTERNPROTO "../protos/Cake.proto" -EXTERNPROTO "../protos/Cherry.proto" EXTERNPROTO "../protos/OpponentBox.proto" EXTERNPROTO "../protos/Judge.proto" EXTERNPROTO "../protos/StaticObjects.proto" @@ -28,8 +26,8 @@ WorldInfo { ] } Viewpoint { - orientation 0 1 0 1.57 - position -0.00035436539785744825 0 5.4449998734478715 + orientation 0.006249707430588943 0.9999401686365654 -0.008977767211780799 1.21630611235842 + position -1.8930923762430245 -0.048865452007669896 5.216846605544976 } TexturedBackground { } @@ -39,13 +37,13 @@ StaticObjects { } DEF ROBOT_OPPONENT_BIG OpponentBox { name "opponent_box_big" - translation -0.775 0.375 0 + translation -0.775 1.195 0 rotation 0 0 1 0 } DEF ROBOT_OPPONENT_SMALL OpponentBox { name "opponent_box_small" - translation 0.21, -1.16, 0 - rotation 0 0 1 1.570 + translation 0.72 1.16 0 + rotation 0 0 1 -1.571595307179586 arucoNumber 2 } DEF JUDGE Judge { @@ -65,379 +63,16 @@ Robot { name "camera_central" controller "" } -Cake { - name "pink_back_left_bottom" - translation -0.775 0.925 0.00992641115425317 - rotation -0.5206212624239871 0.8257866150598635 0.21686393775827936 2.5704198992291135e-16 -} -Cake { - name "pink_back_left_middle" - translation -0.775 0.925 0.02990188631790662 - rotation -0.9107727839774825 -0.22945317756556552 0.34328439415588213 1.9440643868964791e-16 -} -Cake { - name "pink_back_left_top" - translation -0.775 0.925 0.04988962389967098 - rotation -0.896217719418096 0.3118127425763813 0.31554177689811946 2.3433128973680313e-16 -} -Cake { - name "yellow_back_left_bottom" - translation -0.775 0.725 0.00992641115425317 - rotation -0.20030573523425022 0.973883656967124 -0.10690292383563517 2.2269544626059616e-16 - color "yellow" -} -Cake { - name "yellow_back_left_middle" - translation -0.775 0.725 0.029901886317906624 - rotation -0.9457325382119578 -0.10649592892621651 -0.3069993213173446 1.5707352224509273e-16 - color "yellow" -} -Cake { - name "yellow_back_left_top" - translation -0.775 0.725 0.04988962389967098 - rotation -0.3492823423057794 0.9254974310031325 -0.14647986400864318 2.6107279214998543e-16 - color "yellow" -} -Cake { - name "brown_back_left_bottom" - translation -0.275 0.375 0.009926411154253174 - rotation -0.8645670752472675 -0.4995474563250696 0.05455374668653292 1.0318962258373892e-16 - color "brown" -} -Cake { - name "brown_back_left_middle" - translation -0.275 0.375 0.02990188631790662 - rotation 0.3504315774780496 -0.9187493861669371 0.18192656464662268 2.5085230873135394e-16 - color "brown" -} -Cake { - name "brown_back_left_top" - translation -0.275 0.375 0.04988962389967098 - rotation 0.23484475123308782 -0.8007025177996078 0.5511110784657101 8.820757480604414e-17 - color "brown" -} -Cake { - name "pink_back_right_bottom" - translation 0.775 0.925 0.00992641115425317 - rotation -0.5206212624239871 0.8257866150598635 0.21686393775827936 2.5704198992291135e-16 -} -Cake { - name "pink_back_right_middle" - translation 0.775 0.925 0.02990188631790662 - rotation -0.9107727839774825 -0.22945317756556552 0.34328439415588213 1.9440643868964791e-16 -} -Cake { - name "pink_back_right_top" - translation 0.775 0.925 0.04988962389967098 - rotation -0.896217719418096 0.3118127425763813 0.31554177689811946 2.3433128973680313e-16 -} -Cake { - name "yellow_back_right_bottom" - translation 0.775 0.725 0.00992641115425317 - rotation -0.20030573523425022 0.973883656967124 -0.10690292383563517 2.2269544626059616e-16 - color "yellow" -} -Cake { - name "yellow_back_right_middle" - translation 0.775 0.725 0.029901886317906624 - rotation -0.9457325382119578 -0.10649592892621651 -0.3069993213173446 1.5707352224509273e-16 - color "yellow" -} -Cake { - name "yellow_back_right_top" - translation 0.775 0.725 0.04988962389967098 - rotation -0.3492823423057794 0.9254974310031325 -0.14647986400864318 2.6107279214998543e-16 - color "yellow" -} -Cake { - name "brown_back_right_bottom" - translation 0.275 0.375 0.00992641115425317 - rotation 0.2641968251668051 0.9631785759086707 0.049870517165211084 3.3768138635567013e-16 - color "brown" -} -Cake { - name "brown_back_right_middle" - translation 0.275 0.375 0.02990188631790662 - rotation 0.9385294180997202 0.22681621099897425 0.2602247831961226 1.8027772496981338e-16 - color "brown" -} -Cake { - name "brown_back_right_top" - translation 0.275 0.375 0.04988962389967098 - rotation 0.8921894108713864 -0.3803602332766501 0.24356549031154023 1.771358171129791e-16 - color "brown" -} -Cake { - name "pink_front_left_bottom" - translation -0.775 -0.925 0.00992641115425317 - rotation -0.5206212624239871 0.8257866150598635 0.21686393775827936 2.5704198992291135e-16 -} -Cake { - name "pink_front_left_middle" - translation -0.775 -0.925 0.02990188631790662 - rotation -0.9107727839774825 -0.22945317756556552 0.34328439415588213 1.9440643868964791e-16 -} -Cake { - name "pink_front_left_top" - translation -0.775 -0.925 0.04988962389967098 - rotation -0.896217719418096 0.3118127425763813 0.31554177689811946 2.3433128973680313e-16 -} -Cake { - name "yellow_front_left_bottom" - translation -0.775 -0.725 0.00992641115425317 - rotation -0.20030573523425022 0.973883656967124 -0.10690292383563517 2.2269544626059616e-16 - color "yellow" -} -Cake { - name "yellow_front_left_middle" - translation -0.775 -0.725 0.029901886317906624 - rotation -0.9457325382119578 -0.10649592892621651 -0.3069993213173446 1.5707352224509273e-16 - color "yellow" -} -Cake { - name "yellow_front_left_top" - translation -0.775 -0.725 0.04988962389967098 - rotation -0.3492823423057794 0.9254974310031325 -0.14647986400864318 2.6107279214998543e-16 - color "yellow" -} -Cake { - name "brown_front_left_bottom" - translation -0.275 -0.375 0.009926411154253174 - rotation -0.8645670752472675 -0.4995474563250696 0.05455374668653292 1.0318962258373892e-16 - color "brown" -} -Cake { - name "brown_front_left_middle" - translation -0.275 -0.375 0.02990188631790662 - rotation 0.3504315774780496 -0.9187493861669371 0.18192656464662268 2.5085230873135394e-16 - color "brown" -} -Cake { - name "brown_front_left_top" - translation -0.275 -0.375 0.04988962389967098 - rotation 0.23484475123308782 -0.8007025177996078 0.5511110784657101 8.820757480604414e-17 - color "brown" -} -Cake { - name "pink_front_right_bottom" - translation 0.775 -0.925 0.00992641115425317 - rotation -0.5206212624239871 0.8257866150598635 0.21686393775827936 2.5704198992291135e-16 -} -Cake { - name "pink_front_right_middle" - translation 0.775 -0.925 0.02990188631790662 - rotation -0.9107727839774825 -0.22945317756556552 0.34328439415588213 1.9440643868964791e-16 -} -Cake { - name "pink_front_right_top" - translation 0.775 -0.925 0.04988962389967098 - rotation -0.896217719418096 0.3118127425763813 0.31554177689811946 2.3433128973680313e-16 -} -Cake { - name "yellow_front_right_bottom" - translation 0.775 -0.725 0.00992641115425317 - rotation -0.20030573523425022 0.973883656967124 -0.10690292383563517 2.2269544626059616e-16 - color "yellow" -} -Cake { - name "yellow_front_right_middle" - translation 0.775 -0.725 0.029901886317906624 - rotation -0.9457325382119578 -0.10649592892621651 -0.3069993213173446 1.5707352224509273e-16 - color "yellow" -} -Cake { - name "yellow_front_right_top" - translation 0.775 -0.725 0.04988962389967098 - rotation -0.3492823423057794 0.9254974310031325 -0.14647986400864318 2.6107279214998543e-16 - color "yellow" -} -Cake { - name "brown_front_right_bottom" - translation 0.275 -0.375 0.00992641115425317 - rotation 0.2641968251668051 0.9631785759086707 0.049870517165211084 3.3768138635567013e-16 - color "brown" -} -Cake { - name "brown_front_right_middle" - translation 0.275 -0.375 0.02990188631790662 - rotation 0.9385294180997202 0.22681621099897425 0.2602247831961226 1.8027772496981338e-16 - color "brown" -} -Cake { - name "brown_front_right_top" - translation 0.275 -0.375 0.04988962389967098 - rotation 0.8921894108713864 -0.3803602332766501 0.24356549031154023 1.771358171129791e-16 - color "brown" -} -Cherry { - name "cherry_back_0" - translation 0 1.485 0.0351525 -} -Cherry { - name "cherry_back_1" - translation 0 1.455 0.0351525 -} -Cherry { - name "cherry_back_2" - translation 0 1.425 0.0351525 -} -Cherry { - name "cherry_back_3" - translation 0 1.395 0.0351525 -} -Cherry { - name "cherry_back_4" - translation 0 1.365 0.0351525 -} -Cherry { - name "cherry_back_5" - translation 0 1.335 0.0351525 -} -Cherry { - name "cherry_back_6" - translation 0 1.305 0.0351525 -} -Cherry { - name "cherry_back_7" - translation 0 1.275 0.0351525 -} -Cherry { - name "cherry_back_8" - translation 0 1.245 0.0351525 -} -Cherry { - name "cherry_back_9" - translation 0 1.215 0.0351525 -} -Cherry { - name "cherry_front_0" - translation 0 -1.485 0.0351525 -} -Cherry { - name "cherry_front_1" - translation 0 -1.455 0.0351525 -} -Cherry { - name "cherry_front_2" - translation 0 -1.425 0.0351525 -} -Cherry { - name "cherry_front_3" - translation 0 -1.395 0.0351525 -} -Cherry { - name "cherry_front_4" - translation 0 -1.365 0.0351525 -} -Cherry { - name "cherry_front_5" - translation 0 -1.335 0.0351525 -} -Cherry { - name "cherry_front_6" - translation 0 -1.305 0.0351525 -} -Cherry { - name "cherry_front_7" - translation 0 -1.275 0.0351525 -} -Cherry { - name "cherry_front_8" - translation 0 -1.245 0.0351525 -} -Cherry { - name "cherry_front_9" - translation 0 -1.215 0.0351525 -} -Cherry { - name "cherry_left_0" - translation -0.985 0.135 0.0351525 -} -Cherry { - name "cherry_left_1" - translation -0.985 0.105 0.0351525 -} -Cherry { - name "cherry_left_2" - translation -0.985 0.075 0.0351525 -} -Cherry { - name "cherry_left_3" - translation -0.985 0.045 0.0351525 -} -Cherry { - name "cherry_left_4" - translation -0.985 0.015 0.0351525 -} -Cherry { - name "cherry_left_5" - translation -0.985 -0.015 0.0351525 -} -Cherry { - name "cherry_left_6" - translation -0.985 -0.045 0.0351525 -} -Cherry { - name "cherry_left_7" - translation -0.985 -0.075 0.0351525 -} -Cherry { - name "cherry_left_8" - translation -0.985 -0.105 0.0351525 -} -Cherry { - name "cherry_left_9" - translation -0.985 -0.135 0.0351525 -} -Cherry { - name "cherry_right_0" - translation 0.985 0.135 0.0351525 -} -Cherry { - name "cherry_right_1" - translation 0.985 0.105 0.0351525 -} -Cherry { - name "cherry_right_2" - translation 0.985 0.075 0.0351525 -} -Cherry { - name "cherry_right_3" - translation 0.985 0.045 0.0351525 -} -Cherry { - name "cherry_right_4" - translation 0.985 0.015 0.0351525 -} -Cherry { - name "cherry_right_5" - translation 0.985 -0.015 0.0351525 -} -Cherry { - name "cherry_right_6" - translation 0.985 -0.045 0.0351525 -} -Cherry { - name "cherry_right_7" - translation 0.985 -0.075 0.0351525 -} -Cherry { - name "cherry_right_8" - translation 0.985 -0.105 0.0351525 -} -Cherry { - name "cherry_right_9" - translation 0.985 -0.135 0.0351525 -} DEF ROBOT_SMALL GenericRobot { name "robot_small" - translation -0.21 -1.16 0 + translation 0.72 -1.16 0 rotation 0 0 1 1.5708 arucoNumber 6 } DEF ROBOT_BIG GenericRobot { name "robot_big" - translation -0.65 -0.43 0 + translation -0.72 -1.16 0 + rotation 0 0 1 1.5708 arucoNumber 7 baseSlot [ Solid { @@ -451,7 +86,6 @@ DEF ROBOT_BIG GenericRobot { RotationalMotor { name "m6" controlPID 1000 0 0 - minPosition 3.14 maxTorque 1000 } PositionSensor {