Skip to content

Commit ad13110

Browse files
URJalaURJala
committed
Controller will now send over all received points, before sending them to the robot.
1 parent b8ef0a7 commit ad13110

File tree

4 files changed

+76
-61
lines changed

4 files changed

+76
-61
lines changed

‎ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ namespace ur_controllers
6363
{
6464
enum CommandInterfaces
6565
{
66-
PASSTHROUGH_TRAJECTORY_PRESENT = 0,
66+
PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0,
6767
PASSTHROUGH_POINT_WRITTEN = 1,
6868
PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS = 2,
6969
PASSTHROUGH_TRAJECTORY_CANCEL = 3,

‎ur_controllers/src/passthrough_trajectory_controller.cpp‎

Lines changed: 26 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -193,7 +193,7 @@ rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_ca
193193
const std::shared_ptr<rclcpp_action::ServerGoalHandle<control_msgs::action::JointTrajectory>> goal_handle)
194194
{
195195
RCLCPP_INFO(get_node()->get_logger(), "Canceling active trajectory because cancel callback received.");
196-
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(0.0);
196+
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0);
197197
current_point_ = 0;
198198
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(1.0);
199199
std::shared_ptr<control_msgs::action::JointTrajectory::Result> result =
@@ -215,16 +215,7 @@ void PassthroughTrajectoryController::goal_accepted_callback(
215215
active_joint_traj_.points.back().time_from_start.nanosec;
216216
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS].set_value(
217217
active_joint_traj_.points.size());
218-
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(1.0);
219-
for (int i = 0; i < 6; i++) {
220-
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value(
221-
active_joint_traj_.points[current_point_].positions[i]);
222-
}
223-
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value(
224-
active_joint_traj_.points[current_point_].time_from_start.sec +
225-
(active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000));
226-
command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0);
227-
current_point_++;
218+
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(1.0);
228219
std::thread{ std::bind(&PassthroughTrajectoryController::execute, this, std::placeholders::_1), goal_handle }
229220
.detach();
230221
return;
@@ -235,44 +226,50 @@ void PassthroughTrajectoryController::execute(
235226
{
236227
std::cout << "------------------Entered Execute loop--------------------------" << std::endl;
237228
rclcpp::Rate loop_rate(500);
238-
while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].get_value() == 1.0) {
229+
while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) {
239230
if (command_interfaces_[PASSTHROUGH_TRAJECTORY_CANCEL].get_value() == 1.0) {
240231
std::cout << "------------------Entered cancel--------------------------" << std::endl;
241232
std::shared_ptr<control_msgs::action::JointTrajectory::Result> result =
242233
std::make_shared<control_msgs::action::JointTrajectory::Result>();
243234
goal_handle->abort(result);
244-
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(0.0);
235+
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0);
245236
trajectory_active_ = false;
246237
return;
247238
}
239+
248240
if (goal_handle->is_canceling()) {
249241
std::shared_ptr<control_msgs::action::JointTrajectory::Result> result =
250242
std::make_shared<control_msgs::action::JointTrajectory::Result>();
251243
goal_handle->canceled(result);
252-
} else {
253-
// Write a new point to the command interface, if the previous point has been written to the hardware.
254-
if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 &&
255-
current_point_ < active_joint_traj_.points.size()) {
256-
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value(
257-
active_joint_traj_.points[current_point_].time_from_start.sec +
258-
(active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000));
259-
for (int i = 0; i < 6; i++) {
260-
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value(
261-
active_joint_traj_.points[current_point_].positions[i]);
262-
}
263-
command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0);
264-
current_point_++;
244+
return;
245+
}
246+
247+
// Write a new point to the command interface, if the previous point has been written to the hardware interface.
248+
if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 &&
249+
current_point_ < active_joint_traj_.points.size()) {
250+
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value(
251+
active_joint_traj_.points[current_point_].time_from_start.sec +
252+
(active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000));
253+
for (int i = 0; i < 6; i++) {
254+
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value(
255+
active_joint_traj_.points[current_point_].positions[i]);
265256
}
257+
command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0);
258+
current_point_++;
266259
}
267260
// Check if all points have been written to the hardware
268261
if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 &&
269262
current_point_ == active_joint_traj_.points.size() &&
270-
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].get_value() == 1.0) {
271-
RCLCPP_INFO(get_node()->get_logger(), "All points sent to the robot!");
272-
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(0.0);
263+
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) {
264+
RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajecotry will now execute!");
265+
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0);
266+
}
267+
if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) {
273268
std::shared_ptr<control_msgs::action::JointTrajectory::Result> result =
274269
std::make_shared<control_msgs::action::JointTrajectory::Result>();
275270
goal_handle->succeed(result);
271+
command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0);
272+
return;
276273
}
277274
loop_rate.sleep();
278275
}

‎ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -194,7 +194,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
194194
bool initialized_;
195195
double system_interface_initialized_;
196196
bool async_thread_shutdown_;
197-
double passthrough_trajectory_present_;
197+
double passthrough_trajectory_transfer_state_;
198198
double passthrough_trajectory_cancel_;
199199
double passthrough_point_written_;
200200
double passthrough_trajectory_number_of_points_;
@@ -224,6 +224,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
224224
bool non_blocking_read_;
225225
double robot_program_running_copy_;
226226
bool passthrough_trajectory_executing_;
227+
std::vector<std::array<double, 6>> trajectory_joint_positions_;
228+
std::vector<double> trajectory_times_;
227229

228230
PausingState pausing_state_;
229231
double pausing_ramp_up_increment_;

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 46 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -299,8 +299,9 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
299299
command_interfaces.emplace_back(hardware_interface::CommandInterface(
300300
tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_));
301301

302-
command_interfaces.emplace_back(hardware_interface::CommandInterface(
303-
tf_prefix + "passthrough_controller", "passthrough_trajectory_present", &passthrough_trajectory_present_));
302+
command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller",
303+
"passthrough_trajectory_transfer_state",
304+
&passthrough_trajectory_transfer_state_));
304305

305306
command_interfaces.emplace_back(hardware_interface::CommandInterface(
306307
tf_prefix + "passthrough_controller", "passthrough_point_written", &passthrough_point_written_));
@@ -945,40 +946,55 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
945946
void URPositionHardwareInterface::check_passthrough_trajectory_controller()
946947
{
947948
static double last_time = 0.0;
948-
if (passthrough_trajectory_present_ == 1.0) {
949-
if (!passthrough_trajectory_executing_) {
950-
ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
951-
static_cast<int>(passthrough_trajectory_number_of_points_));
952-
passthrough_trajectory_executing_ = true;
953-
std::cout << "Received points to passthrough: " << passthrough_trajectory_number_of_points_ << std::endl;
949+
if (passthrough_trajectory_transfer_state_ == 1.0) {
950+
trajectory_joint_positions_.push_back(passthrough_trajectory_positions_);
951+
trajectory_times_.push_back(passthrough_trajectory_time_from_start_ - last_time);
952+
last_time = passthrough_trajectory_time_from_start_;
953+
passthrough_point_written_ = 1.0;
954+
} else if (passthrough_trajectory_transfer_state_ == 2.0) {
955+
ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
956+
trajectory_joint_positions_.size());
957+
for (int i = 0; i < trajectory_joint_positions_.size(); i++) {
958+
ur_driver_->writeTrajectoryPoint(trajectory_joint_positions_[i], false, trajectory_times_[i]);
954959
}
955-
if (passthrough_trajectory_executing_ && passthrough_point_written_ == 0.0) {
956-
std::cout << "Writing point to robot with time parameter: " << passthrough_trajectory_time_from_start_
957-
<< std::endl;
958-
959-
bool status = ur_driver_->writeTrajectoryPoint(passthrough_trajectory_positions_, false,
960-
passthrough_trajectory_time_from_start_ - last_time);
961-
962-
std::cout << "Status of write: " << status << std::endl;
963-
if (!status) {
964-
std::cout << "Write failed, cancelling trajectory" << std::endl;
965-
passthrough_trajectory_cancel_ = 1.0;
966-
std::cout << "----------------------------Cancelling trajectory in hardware interface------------------"
967-
<< std::endl;
968-
ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL);
969-
} else {
970-
last_time = passthrough_trajectory_time_from_start_;
971-
passthrough_point_written_ = 1.0;
972-
}
973-
}
974-
} else {
975-
passthrough_trajectory_executing_ = false;
976-
}
960+
passthrough_trajectory_transfer_state_ = 3.0;
961+
}
962+
963+
// if (passthrough_trajectory_transfer_state_ == 1.0) {
964+
// if (!passthrough_trajectory_executing_) {
965+
// ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
966+
// static_cast<int>(passthrough_trajectory_number_of_points_));
967+
// passthrough_trajectory_executing_ = true;
968+
// std::cout << "Received points to passthrough: " << passthrough_trajectory_number_of_points_ << std::endl;
969+
// }
970+
// if (passthrough_trajectory_executing_ && passthrough_point_written_ == 0.0) {
971+
// std::cout << "Writing point to robot with time parameter: " << passthrough_trajectory_time_from_start_
972+
// << std::endl;
973+
974+
// bool status = ur_driver_->writeTrajectoryPoint(passthrough_trajectory_positions_, false,
975+
// passthrough_trajectory_time_from_start_ - last_time);
976+
977+
// std::cout << "Status of write: " << status << std::endl;
978+
// if (!status) {
979+
// std::cout << "Write failed, cancelling trajectory" << std::endl;
980+
// passthrough_trajectory_cancel_ = 1.0;
981+
// std::cout << "----------------------------Cancelling trajectory in hardware interface------------------"
982+
// << std::endl;
983+
// ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL);
984+
// } else {
985+
// last_time = passthrough_trajectory_time_from_start_;
986+
// passthrough_point_written_ = 1.0;
987+
// }
988+
// }
989+
// } else {
990+
// passthrough_trajectory_executing_ = false;
991+
// }
977992
}
978993

979994
void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result)
980995
{
981996
std::cout << "-------------------Triggered trajectory callback!--------------------------" << std::endl;
997+
passthrough_trajectory_transfer_state_ = 4.0;
982998
std::cout << "Result is: " << int(result) << std::endl;
983999
passthrough_trajectory_executing_ = false;
9841000
return;

0 commit comments

Comments
 (0)