Nantes Université

Skip to content
Extraits de code Groupes Projets

Comparer les révisions

Les modifications sont affichées comme si la révision source était fusionnée avec la révision cible. En savoir plus sur la comparaison des révisions.

Source

Sélectionner le projet cible
No results found

Cible

Sélectionner le projet cible
  • ls2n-drones/ls2n_drone_armada
1 résultat
Afficher les modifications
Validations sur la source (3)
Masquer les modifications d'espaces
En ligne Côte à côte
Affichage de
avec 218 ajouts et 104 suppressions
CMakeLists.txt
Voir le fichier @ 96a00c4d
......@@ -25,9 +25,6 @@ add_custom_target(
COMMAND bash scripts/generate_sdf.sh ${CMAKE_INSTALL_PREFIX}
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
# Install Python modules
ament_python_install_package(${PROJECT_NAME})
# Install Python executables
install(
PROGRAMS ${PROJECT_NAME}/mc_node.py ${PROJECT_NAME}/odometry_transformer.py
......
armada_controller/CMakeLists.txt
Voir le fichier @ 96a00c4d
......@@ -6,9 +6,16 @@ find_package(std_srvs REQUIRED)
find_package(ls2n_drone_interfaces REQUIRED)
find_package(trajectory_msgs REQUIRED)
find_package(rcl_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# Generate services
rosidl_generate_interfaces(ls2n_drone_armada
"srv/MoveUp.srv"
DEPENDENCIES std_msgs
)
# Create a library for the controller core
add_library(armada_controller_core src/armada_controller.cpp)
......@@ -32,6 +39,10 @@ target_include_directories(armada_controller PRIVATE include
# Link the executable to the core library
target_link_libraries(armada_controller armada_controller_core Eigen3::Eigen)
# Link with the generated services
rosidl_get_typesupport_target(cpp_typesupport_target ls2n_drone_armada "rosidl_typesupport_cpp")
target_link_libraries(armada_controller "${cpp_typesupport_target}")
# ROS2 dependencies for the executable
ament_target_dependencies(
armada_controller
......
armada_controller/include/armada_controller/armada_controller_node.hpp
Voir le fichier @ 96a00c4d
......@@ -3,6 +3,7 @@
#include "armada_controller/armada_controller.hpp"
#include "ls2n_drone_armada/srv/move_up.hpp"
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <ls2n_drone_interfaces/srv/controller_selection.hpp>
......@@ -136,9 +137,37 @@ private:
double initial_time_;
double previous_loop_time_ = 0.0;
/**
* @brief Service callback to move the payload up
* @param request MoveUp request with distance and speed
* @param response MoveUp response with success status
*/
void moveUpCallback(
const std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Request> request,
std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Response> response);
/**
* @brief Timer callback for updating z position during move up operation
*/
void moveUpTimerCallback();
// Controller state
bool controller_active_ = false;
bool simulation_mode_ = false;
// Move up operation state
bool move_up_active_ = false;
Eigen::Vector3d move_up_start_position_;
double move_up_target_distance_ = 0.0;
double move_up_speed_ = 0.0;
double move_up_start_time_ = 0.0;
double move_up_current_distance_ = 0.0;
// Timer for move up operation
rclcpp::TimerBase::SharedPtr move_up_timer_;
// Service for move up operation
rclcpp::Service<ls2n_drone_armada::srv::MoveUp>::SharedPtr move_up_service_;
};
#endif // ARMADA_CONTROLLER_NODE_HPP_
armada_controller/src/armada_controller_node.cpp
Voir le fichier @ 96a00c4d
......@@ -120,6 +120,16 @@ ArmadaControllerNode::ArmadaControllerNode() : Node("armada_controller") {
std::shared_ptr<std_srvs::srv::Empty::Response> response) {
this->activateArmadaControl(request, response);
});
// Add move up service
move_up_service_ = create_service<ls2n_drone_armada::srv::MoveUp>(
"/Armada/MoveUp",
[this](
const std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Request>
request,
std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Response> response) {
this->moveUpCallback(request, response);
});
}
void ArmadaControllerNode::droneOdometryCallback(
......@@ -360,6 +370,108 @@ void ArmadaControllerNode::sendDesiredForces(
}
}
void ArmadaControllerNode::moveUpCallback(
const std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Request> request,
std::shared_ptr<ls2n_drone_armada::srv::MoveUp::Response> response) {
RCLCPP_INFO(get_logger(), "Move up request: distance=%.2f, speed=%.2f",
request->distance, request->speed);
// Check if controller is active
if (!controller_active_) {
response->success = false;
response->message = "Armada controller is not active";
RCLCPP_ERROR(get_logger(), "Cannot move up: Armada controller not active");
return;
}
// Check if we already have a move up operation in progress
if (move_up_active_) {
response->success = false;
response->message = "Move up operation already in progress";
RCLCPP_ERROR(get_logger(),
"Cannot start move up: operation already in progress");
return;
}
// Check parameters validity
if (request->distance <= 0.0) {
response->success = false;
response->message = "Distance must be positive";
RCLCPP_ERROR(get_logger(), "Invalid move up distance: %.2f",
request->distance);
return;
}
if (request->speed <= 0.0) {
response->success = false;
response->message = "Speed must be positive";
RCLCPP_ERROR(get_logger(), "Invalid move up speed: %.2f", request->speed);
return;
}
// Store current position
move_up_start_position_ = payload_state_.position;
move_up_target_distance_ = request->distance;
move_up_speed_ = request->speed;
move_up_start_time_ = this->now().seconds();
move_up_current_distance_ = 0.0;
move_up_active_ = true;
// Create timer for 100Hz updates (10ms period)
move_up_timer_ = this->create_wall_timer(
std::chrono::milliseconds(10),
std::bind(&ArmadaControllerNode::moveUpTimerCallback, this));
response->success = true;
response->message = "Move up operation started";
RCLCPP_INFO(get_logger(),
"Move up operation started: target=%.2f, speed=%.2f",
request->distance, request->speed);
}
void ArmadaControllerNode::moveUpTimerCallback() {
// Check if we're still active
if (!move_up_active_ || !controller_active_) {
move_up_timer_->cancel();
move_up_active_ = false;
RCLCPP_INFO(get_logger(), "Move up operation stopped");
return;
}
// Calculate elapsed time
double current_time = this->now().seconds();
double elapsed_time = current_time - move_up_start_time_;
// Calculate new distance based on speed and time
move_up_current_distance_ = move_up_speed_ * elapsed_time;
// Check if we've reached the target
if (move_up_current_distance_ >= move_up_target_distance_) {
move_up_current_distance_ = move_up_target_distance_;
move_up_active_ = false;
move_up_timer_->cancel();
RCLCPP_INFO(get_logger(), "Move up operation completed");
}
// Update desired position
Eigen::Vector3d new_desired_position = move_up_start_position_;
new_desired_position.z() += move_up_current_distance_;
// Update desired velocity (only in z direction)
Eigen::Vector3d new_desired_velocity = Eigen::Vector3d::Zero();
if (move_up_active_) {
new_desired_velocity.z() = move_up_speed_;
}
// Update desired state
desired_state_.segment<3>(0) = new_desired_position;
desired_state_velocity_.segment<3>(0) = new_desired_velocity;
RCLCPP_DEBUG(get_logger(), "Move up progress: %.2f/%.2f meters",
move_up_current_distance_, move_up_target_distance_);
}
void ArmadaControllerNode::sendCurrentAndDesiredConfig() {
// Get current state
const auto &current_state = controller_->getCurrentState();
......
armada_controller/srv/MoveUp.srv 0 → 100644
Voir le fichier @ 96a00c4d
# Request - parameters for moving up
float64 distance # The distance to move up in meters
float64 speed # The speed at which to move in meters/second
---
# Response
bool success # Whether the command was accepted
string message # Additional information
launch/sitl_armada_control.launch.py
Voir le fichier @ 96a00c4d
......@@ -251,7 +251,7 @@ def generate_launch_description() -> LaunchDescription:
cable_length_launch_arg = DeclareLaunchArgument(
"cable_length",
default_value="1.25",
default_value="3.0",
description="Length of a cable [m]",
)
......
models/armada_net/armada_net.erb
Voir le fichier @ 96a00c4d
<%
l_body = 1.25 # length of the body you need also to change the drone/support position in world/default.sdf
l_body = 3.0 # length of the body you need also to change the drone/support position in world/default.sdf
r_body = 0.0025 # radius of the body in m
n_part = 5.0 # number of sub-parts to simulate a flexible body
n_part = 6.0 # number of sub-parts to simulate a flexible body
l_part = l_body/n_part
m_body = 0.01 # global mass of body
m_part = m_body/n_part
......@@ -239,24 +239,10 @@
<!-- Joints connecting platform to cables -->
<% num_cables.times do |i| %>
<joint name="pf_joint<%= i %>" type="universal">
<joint name="pf_joint<%= i %>" type="ball">
<parent>platform</parent>
<child>cable<%= i %>::body_0_0_cable<%= i %></child>
<pose>0 0 <%= -l_part_0/2 %> 0 0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<damping><%= damping %></damping>
<spring_stiffness><%= stiffness %></spring_stiffness>
</dynamics>
</axis>
<axis2>
<xyz>0 1 0</xyz>
<dynamics>
<damping><%= damping %></damping>
<spring_stiffness><%= stiffness %></spring_stiffness>
</dynamics>
</axis2>
</joint>
<% end %>
......@@ -309,23 +295,6 @@
<izz><%= node_izz %></izz>
</inertia>
</inertial>
<collision name='net_node<%= n %>_cable<%= i %>_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius><%= net_radius %></radius>
</sphere>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
<collide_bitmask>0x1000</collide_bitmask>
</contact>
</surface>
</collision>
<!-- Visual -->
<visual name="net_node<%= n %>_cable<%= i %>_visual">
<geometry>
......@@ -349,23 +318,6 @@
<izz><%= node_izz %></izz>
</inertia>
</inertial>
<collision name='net_node<%= n %>_cable<%= i %>_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius><%= net_radius %></radius>
</sphere>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
<collide_bitmask>0x1000</collide_bitmask>
</contact>
</surface>
</collision>
<!-- Visual -->
<visual name="net_node<%= n %>_cable<%= i %>_visual">
<geometry>
......@@ -389,23 +341,6 @@
<izz><%= node_izz %></izz>
</inertia>
</inertial>
<collision name='net_node<%= n %>_cable<%= i %>_collision'>
<pose>0 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius><%= net_radius %></radius>
</sphere>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
<collide_bitmask>0x1000</collide_bitmask>
</contact>
</surface>
</collision>
<!-- Visual -->
<visual name="net_node<%= n %>_cable<%= i %>_visual">
<geometry>
......@@ -500,43 +435,15 @@
</geometry>
</visual>
</link>
<joint name="net_joint<%= n %>_<%= i %>_a" type="universal">
<joint name="net_joint<%= n %>_<%= i %>_a" type="ball">
<child>net_link<%= n %>_cable<%= i %>_a</child>
<parent>net_node<%= n %>_cable<%= i %></parent>
<pose>0 0 <%= -l_attach / 2.0 %> 0 0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<damping><%= damping %></damping>
<spring_stiffness><%= stiffness %></spring_stiffness>
</dynamics>
</axis>
<axis2>
<xyz>0 1 0</xyz>
<dynamics>
<damping><%= damping %></damping>
<spring_stiffness><%= stiffness %></spring_stiffness>
</dynamics>
</axis2>
</joint>
<joint name="net_joint<%= n %>_<%= i %>_b" type="universal">
<joint name="net_joint<%= n %>_<%= i %>_b" type="ball">
<child>net_link<%= n %>_cable<%= i %>_b</child>
<parent>net_node<%= n %>_cable<%= i %></parent>
<pose>0 0 <%= l_attach / 2.0 %> 0 0 0</pose>
<axis>
<xyz>1 0 0</xyz>
<dynamics>
<damping><%= damping %></damping>
<spring_stiffness><%= stiffness %></spring_stiffness>
</dynamics>
</axis>
<axis2>
<xyz>0 1 0</xyz>
<dynamics>
<damping><%= damping %></damping>
<spring_stiffness><%= stiffness %></spring_stiffness>
</dynamics>
</axis2>
</joint>
<joint name="net_joint<%= n %>_<%= i %>_c" type="universal">
......
package.xml
Voir le fichier @ 96a00c4d
......@@ -9,8 +9,12 @@
<license>Apache 2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>gz_attach_links</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<exec_depend>ls2n_drone_bridge</exec_depend>
<exec_depend>ls2n_drone_controllers</exec_depend>
......
worlds/armada_world_net.erb
Voir le fichier @ 96a00c4d
<%
l_cable = 1.25 # length of the cable you need also to change the drone/support position in world/default.sdf
n_body = 5
l_cable = 3 # length of the cable you need also to change the drone/support position in world/default.sdf
n_body = 6
num_drones = 4
net_size = 0.3
ori = Math::PI/2.0 - 0.5
......@@ -158,6 +158,52 @@
<self_collide>false</self_collide>
</model>
<!-- 10cm Cube -->
<model name="test_cube">
<pose>0 0 0.1 0 0 0</pose>
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>0.2 0.2 0.2</size>
</box>
</geometry>
<surface>
<contact>
<collide_bitmask>0x1ffff</collide_bitmask>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0</max_vel>
</ode>
</contact>
</surface>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.2 0.2 0.2</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>0.000167</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000167</iyy>
<iyz>0</iyz>
<izz>0.000167</izz>
</inertia>
</inertial>
</link>
</model>
<model name="armada">
<pose>0 0 -0.5 0 0 0</pose>
<include merge="true">
......