Skip to content

Commit

Permalink
Use control simulation in yp-spur instead of ypspur_ros (#120)
Browse files Browse the repository at this point in the history 
Use yp-spur 1.21.0 or later.
  • Loading branch information
@at-wat
at-wat authored Mar 26, 2024
1 parent 486fe1c commit 7d429e0
Show file tree
Hide file tree
Showing 4 changed files with 47 additions and 149 deletions.
2 changes: 1 addition & 1 deletion package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,5 +27,5 @@
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>trajectory_msgs</depend>
<depend version_gte="1.20.0">ypspur</depend>
<depend version_gte="1.21.0">ypspur</depend>
</package>
161 changes: 38 additions & 123 deletions src/ypspur_ros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,6 @@ class YpspurRosNode
std::map<std::string, double> params_;
int key_;
bool simulate_;
bool simulate_control_;

double tf_time_offset_;

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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_))
Expand All @@ -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;
Expand Down Expand Up @@ -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_))
{
Expand Down
29 changes: 7 additions & 22 deletions test/src/joint_trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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<trajectory_msgs::JointTrajectory>("joint_trajectory", 1, true);
ros::Publisher pub_clock =
nh.advertise<rosgraph_msgs::Clock>("clock", 100);

sensor_msgs::JointState::ConstPtr joint_states;
const boost::function<void(const sensor_msgs::JointState::ConstPtr&)> cb_joint =
Expand All @@ -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)
Expand All @@ -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);
Expand All @@ -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();
}
Expand All @@ -102,31 +91,27 @@ 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();
}
ASSERT_NEAR(joint_states->velocity[0], 0.0, 0.1)
<< "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();
}
Expand Down
4 changes: 1 addition & 3 deletions test/test/joint_trajectory.test
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,10 @@
<?xml version="1.0"?>
<launch>
<param name="use_sim_time" value="true" />

<test test-name="test_joint_trajectory" pkg="ypspur_ros" type="test_joint_trajectory" />

<node pkg="ypspur_ros" type="ypspur_ros" name="ypspur_ros">
<param name="compatible" value="1" />
<param name="simulate_control" value="true" />
<param name="simulate" value="true" />
<param name="param_file" value="$(find ypspur_ros)/test/config/test.param" />
<param name="joint0_enable" value="true" />
<param name="hz" value="20.0" />
Expand Down

0 comments on commit 7d429e0

Please sign in to comment.