From 7d429e0426a6a61f067d23c028976dfb89b9cc4c Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Tue, 26 Mar 2024 09:33:03 +0900 Subject: [PATCH] Use control simulation in yp-spur instead of ypspur_ros (#120) Use yp-spur 1.21.0 or later. --- package.xml | 2 +- src/ypspur_ros.cpp | 161 ++++++++------------------------ test/src/joint_trajectory.cpp | 29 ++---- test/test/joint_trajectory.test | 4 +- 4 files changed, 47 insertions(+), 149 deletions(-) diff --git a/package.xml b/package.xml index de1db2d..c07f3e1 100644 --- a/package.xml +++ b/package.xml @@ -27,5 +27,5 @@ tf2_geometry_msgs tf2_ros trajectory_msgs - ypspur + ypspur diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp index be67ccf..77abfdc 100644 --- a/src/ypspur_ros.cpp +++ b/src/ypspur_ros.cpp @@ -97,7 +97,6 @@ class YpspurRosNode std::map params_; int key_; bool simulate_; - bool simulate_control_; double tf_time_offset_; @@ -445,9 +444,13 @@ class YpspurRosNode pnh_.param("port", port_, std::string("/dev/ttyACM0")); pnh_.param("ipc_key", key_, 28741); pnh_.param("simulate", simulate_, false); - pnh_.param("simulate_control", simulate_control_, false); - if (simulate_control_) + bool simulate_control; + pnh_.param("simulate_control", simulate_control, false); + if (simulate_control) + { + ROS_WARN("simulate_control parameter is deprecated. Use simulate parameter instead"); simulate_ = true; + } pnh_.param("ypspur_bin", ypspur_bin_, std::string("ypspur-coordinator")); pnh_.param("param_file", param_file_, std::string("")); pnh_.param("tf_time_offset", tf_time_offset_, 0.0); @@ -848,25 +851,10 @@ class YpspurRosNode double x, y, yaw, v(0), w(0); double t; - if (!simulate_control_) - { - t = YP::YPSpur_get_pos(YP::CS_BS, &x, &y, &yaw); - if (t <= 0.0) - break; - YP::YPSpur_get_vel(&v, &w); - } - else - { - t = ros::Time::now().toSec(); - if (cmd_vel_) - { - v = cmd_vel_->linear.x; - w = cmd_vel_->angular.z; - } - yaw = tf2::getYaw(odom.pose.pose.orientation) + dt * w; - x = odom.pose.pose.position.x + dt * v * cosf(yaw); - y = odom.pose.pose.position.y + dt * v * sinf(yaw); - } + t = YP::YPSpur_get_pos(YP::CS_BS, &x, &y, &yaw); + if (t <= 0.0) + break; + YP::YPSpur_get_vel(&v, &w); const ros::Time current_stamp(t); if (!avoid_publishing_duplicated_odom_ || (current_stamp > previous_odom_stamp_)) @@ -893,12 +881,10 @@ class YpspurRosNode } previous_odom_stamp_ = current_stamp; - if (!simulate_control_) - { - t = YP::YPSpur_get_force(&wrench.wrench.force.x, &wrench.wrench.torque.z); - if (t <= 0.0) - break; - } + t = YP::YPSpur_get_force(&wrench.wrench.force.x, &wrench.wrench.torque.z); + if (t <= 0.0) + break; + wrench.header.stamp = ros::Time(t); wrench.wrench.force.y = 0; wrench.wrench.force.z = 0; @@ -931,109 +917,38 @@ class YpspurRosNode if (joints_.size() > 0) { double t; - if (!simulate_control_) + t = -1.0; + while (t < 0.0) { - t = -1.0; - while (t < 0.0) + int i = 0; + for (auto& j : joints_) { - int i = 0; - for (auto& j : joints_) - { - const double t0 = YP::YP_get_joint_ang(j.id_, &joint.position[i]); - const double t1 = YP::YP_get_joint_vel(j.id_, &joint.velocity[i]); - const double t2 = YP::YP_get_joint_torque(j.id_, &joint.effort[i]); + const double t0 = YP::YP_get_joint_ang(j.id_, &joint.position[i]); + const double t1 = YP::YP_get_joint_vel(j.id_, &joint.velocity[i]); + const double t2 = YP::YP_get_joint_torque(j.id_, &joint.effort[i]); - if (t0 != t1 || t1 != t2) - { - // Retry if updated during this joint - t = -1.0; - break; - } - if (t < 0.0) - { - t = t0; - } - else if (t != t0) - { - // Retry if updated during loops - t = -1.0; - break; - } - i++; + if (t0 != t1 || t1 != t2) + { + // Retry if updated during this joint + t = -1.0; + break; } - } - if (t <= 0.0) - break; - joint.header.stamp = ros::Time(t); - } - else - { - t = ros::Time::now().toSec(); - for (unsigned int i = 0; i < joints_.size(); i++) - { - auto vel_prev = joint.velocity[i]; - switch (joints_[i].control_) + if (t < 0.0) { - case JointParams::STOP: - break; - case JointParams::TRAJECTORY: - case JointParams::POSITION: - case JointParams::VELOCITY: - switch (joints_[i].control_) - { - case JointParams::POSITION: - { - float position_err = joints_[i].angle_ref_ - joint.position[i]; - joints_[i].vel_ref_ = sqrtf(2.0 * joints_[i].accel_ * fabs(position_err)); - if (joints_[i].vel_ref_ > joints_[i].vel_) - joints_[i].vel_ref_ = joints_[i].vel_; - if (position_err < 0) - joints_[i].vel_ref_ = -joints_[i].vel_ref_; - } - break; - case JointParams::TRAJECTORY: - { - float position_err = joints_[i].angle_ref_ - joint.position[i]; - float v_sq = joints_[i].vel_end_ * joints_[i].vel_end_ + 2.0 * joints_[i].accel_ * position_err; - joints_[i].vel_ref_ = sqrtf(fabs(v_sq)); - - float vel_max; - if (fabs(joints_[i].vel_) < fabs(joints_[i].vel_end_)) - { - if (fabs(position_err) < - (joints_[i].vel_end_ * joints_[i].vel_end_ - joints_[i].vel_ * joints_[i].vel_) / - (2.0 * joints_[i].accel_)) - vel_max = fabs(joints_[i].vel_end_); - else - vel_max = joints_[i].vel_; - } - else - vel_max = joints_[i].vel_; - - if (joints_[i].vel_ref_ > vel_max) - joints_[i].vel_ref_ = vel_max; - if (position_err < 0) - joints_[i].vel_ref_ = -joints_[i].vel_ref_; - } - break; - default: - break; - } - joint.velocity[i] = joints_[i].vel_ref_; - if (joint.velocity[i] < vel_prev - dt * joints_[i].accel_) - { - joint.velocity[i] = vel_prev - dt * joints_[i].accel_; - } - else if (joint.velocity[i] > vel_prev + dt * joints_[i].accel_) - { - joint.velocity[i] = vel_prev + dt * joints_[i].accel_; - } - joint.position[i] += joint.velocity[i] * dt; - break; + t = t0; } + else if (t != t0) + { + // Retry if updated during loops + t = -1.0; + break; + } + i++; } - joint.header.stamp = ros::Time(t); } + if (t <= 0.0) + break; + joint.header.stamp = ros::Time(t); if (!avoid_publishing_duplicated_joints_ || (joint.header.stamp > previous_joints_stamp_)) { diff --git a/test/src/joint_trajectory.cpp b/test/src/joint_trajectory.cpp index ce66ab8..d529a84 100644 --- a/test/src/joint_trajectory.cpp +++ b/test/src/joint_trajectory.cpp @@ -10,8 +10,8 @@ * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" @@ -37,14 +37,11 @@ TEST(JointTrajectory, CommandValidation) { - ros::WallDuration wait(0.005); - ros::Duration clock_step(0.05); + ros::WallDuration wait(0.05); ros::NodeHandle nh; ros::Publisher pub_cmd = nh.advertise("joint_trajectory", 1, true); - ros::Publisher pub_clock = - nh.advertise("clock", 100); sensor_msgs::JointState::ConstPtr joint_states; const boost::function cb_joint = @@ -55,15 +52,9 @@ TEST(JointTrajectory, CommandValidation) ros::Subscriber sub_joint_states = nh.subscribe("joint_states", 100, cb_joint); - rosgraph_msgs::Clock clock; - clock.clock.fromNSec(ros::WallTime::now().toNSec()); - pub_clock.publish(clock); - // Wait until ypspur_ros - for (int i = 0; i < 200 * 30; ++i) + for (int i = 0; i < 20 * 30; ++i) { - clock.clock += clock_step; - pub_clock.publish(clock); wait.sleep(); ros::spinOnce(); if (joint_states) @@ -73,7 +64,7 @@ TEST(JointTrajectory, CommandValidation) // Publish valid command trajectory_msgs::JointTrajectory cmd; - cmd.header.stamp = clock.clock; + cmd.header.stamp = ros::Time::now(); cmd.joint_names.resize(1); cmd.joint_names[0] = "joint0"; cmd.points.resize(1); @@ -87,8 +78,6 @@ TEST(JointTrajectory, CommandValidation) for (int i = 0; i < 50; ++i) { - clock.clock += clock_step; - pub_clock.publish(clock); wait.sleep(); ros::spinOnce(); } @@ -102,15 +91,13 @@ TEST(JointTrajectory, CommandValidation) << "Valid joint_trajectory must not be ignored"; // Stop - cmd.header.stamp = clock.clock; + cmd.header.stamp = ros::Time::now(); cmd.points[0].positions[0] = 0.0; cmd.points[0].velocities[0] = 0.0; pub_cmd.publish(cmd); wait.sleep(); for (int i = 0; i < 50; ++i) { - clock.clock += clock_step; - pub_clock.publish(clock); wait.sleep(); ros::spinOnce(); } @@ -118,15 +105,13 @@ TEST(JointTrajectory, CommandValidation) << "Valid joint_trajectory must not be ignored"; // Publish outdated command - cmd.header.stamp = clock.clock - ros::Duration(2.0); + cmd.header.stamp = ros::Time::now() - ros::Duration(2.0); cmd.points[0].positions[0] = 1.0; cmd.points[0].velocities[0] = 1.0; pub_cmd.publish(cmd); wait.sleep(); for (int i = 0; i < 50; ++i) { - clock.clock += clock_step; - pub_clock.publish(clock); wait.sleep(); ros::spinOnce(); } diff --git a/test/test/joint_trajectory.test b/test/test/joint_trajectory.test index 4bf7d59..87d37df 100644 --- a/test/test/joint_trajectory.test +++ b/test/test/joint_trajectory.test @@ -1,12 +1,10 @@ - - - +