diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4021de7..13216b3 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -51,7 +51,8 @@ jobs: rosdep install --ignore-src --from-paths . -y -r colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install source install/setup.bash - colcon test --event-handlers console_direct+ + DEPS_PKG_NAMES=$(colcon list -n --base-paths src/external) + colcon test --event-handlers console_direct+ console_package_list+ --packages-skip $DEPS_PKG_NAMES colcon test-result - name: Upload Tests to Artifacts diff --git a/cartesian_controllers_ros2.repos b/cartesian_controllers_ros2.repos index f967445..e78b80e 100644 --- a/cartesian_controllers_ros2.repos +++ b/cartesian_controllers_ros2.repos @@ -7,4 +7,4 @@ repositories: external/ros-controls/kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git - version: 1.1.0 + version: master diff --git a/cartesian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp b/cartesian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp index 54fb639..34b62eb 100644 --- a/cartesian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp +++ b/cartesian_state_broadcaster/include/cartesian_state_broadcaster/cartesian_state_broadcaster.hpp @@ -21,7 +21,7 @@ #include #include -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "cartesian_state_broadcaster/visibility_control.h" #include "controller_interface/controller_interface.hpp" diff --git a/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp b/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp index 55d8fd3..182ad5b 100644 --- a/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp +++ b/cartesian_state_broadcaster/src/cartesian_state_broadcaster.cpp @@ -53,6 +53,16 @@ CallbackReturn CartesianStateBroadcaster::on_configure( auto kinematics_plugin_package = get_node()->get_parameter("kinematics_plugin_package").as_string(); + // Load URDF + auto robot_param = rclcpp::Parameter(); + if (!get_node()->get_parameter("robot_description", robot_param)) { + RCLCPP_ERROR( + rclcpp::get_logger("CartesianStateBroadcaster"), + "parameter robot_description not set"); + return CallbackReturn::ERROR; + } + auto robot_description = robot_param.as_string(); + // Load the differential IK plugin if (!kinematics_plugin_name.empty()) { try { @@ -62,8 +72,7 @@ CallbackReturn CartesianStateBroadcaster::on_configure( kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(kinematics_plugin_name)); if (!kinematics_->initialize( - get_node()->get_node_parameters_interface(), - end_effector_name_)) + robot_description, get_node()->get_node_parameters_interface(), "kinematics")) { RCLCPP_ERROR( rclcpp::get_logger("CartesianStateBroadcaster"), diff --git a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp index e2d161e..c3c42d2 100644 --- a/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp +++ b/cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_controller.hpp @@ -38,7 +38,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "semantic_components/force_torque_sensor.hpp" // Ros msgs diff --git a/cartesian_vic_controller/src/cartesian_vic_rule.cpp b/cartesian_vic_controller/src/cartesian_vic_rule.cpp index 9767fb3..f29a61b 100644 --- a/cartesian_vic_controller/src/cartesian_vic_rule.cpp +++ b/cartesian_vic_controller/src/cartesian_vic_rule.cpp @@ -61,6 +61,15 @@ CartesianVicRule::configure( num_joints_ = num_joints; // reset vic state reset(num_joints); + + // Load URDF + auto robot_param = rclcpp::Parameter(); + if (!parameters_interface->get_parameter("robot_description", robot_param)) { + RCLCPP_ERROR(logger_, "parameter robot_description not set"); + return controller_interface::return_type::ERROR; + } + auto robot_description = robot_param.as_string(); + // Load the dynamics (also used for IK) plugin if (!parameters_.dynamics.plugin_name.empty()) { try { @@ -70,7 +79,9 @@ CartesianVicRule::configure( "dynamics_interface::DynamicsInterface"); dynamics_ = std::unique_ptr( dynamics_loader_->createUnmanagedInstance(parameters_.dynamics.plugin_name)); - if (!dynamics_->initialize(parameters_interface, parameters_.dynamics.tip)) { + if (!dynamics_->initialize( + robot_description, parameters_interface, "dynamics")) + { return controller_interface::return_type::ERROR; } } catch (pluginlib::PluginlibException & ex) { diff --git a/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp b/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp index 04ff5f0..cbb7496 100644 --- a/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp +++ b/cartesian_vic_servo/include/cartesian_vic_servo/vic_servo_node.hpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" diff --git a/cartesian_vic_servo/src/demo_twist.cpp b/cartesian_vic_servo/src/demo_twist.cpp index f2205f7..768583c 100644 --- a/cartesian_vic_servo/src/demo_twist.cpp +++ b/cartesian_vic_servo/src/demo_twist.cpp @@ -101,7 +101,7 @@ int main(int argc, char * argv[]) // create command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window; - KinematicState current_state = servo.getCurrentRobotState(); + KinematicState current_state = servo.getCurrentRobotState(false); updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now()); diff --git a/cartesian_vic_servo/src/vic_servo_node.cpp b/cartesian_vic_servo/src/vic_servo_node.cpp index f960acd..67de4f6 100644 --- a/cartesian_vic_servo/src/vic_servo_node.cpp +++ b/cartesian_vic_servo/src/vic_servo_node.cpp @@ -271,7 +271,7 @@ bool CartesianVicServo::start() } // Reset command queue - auto current_state = servo_->getCurrentRobotState(); + auto current_state = servo_->getCurrentRobotState(false); moveit_servo::updateSlidingWindow( current_state, joint_cmd_rolling_window_, max_expected_latency_, this->now()); @@ -651,7 +651,7 @@ bool CartesianVicServo::send_twist_command( } else { // if joint_cmd_rolling_window_ is empty or all commands are outdated, use current robot state joint_cmd_rolling_window_.clear(); - current_state = servo_->getCurrentRobotState(); + current_state = servo_->getCurrentRobotState(false); current_state.velocities *= 0.0; } diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp index 0ff6a23..1c5a2a1 100644 --- a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp @@ -29,7 +29,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_publisher.hpp" // Base class for VIC controller #include "cartesian_vic_controller/cartesian_vic_controller.hpp"