Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[jog_arm] Fix valid cmd flags #2013

Merged
merged 2 commits into from
Apr 8, 2020
Merged

[jog_arm] Fix valid cmd flags #2013

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
408bfcd
Rename the 'zero command flag' variables for readability
AndyZe Apr 6, 2020
83b46b7
Remove debug line, clang format
AndyZe Apr 6, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
8 changes: 4 additions & 4 deletions moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,11 +68,11 @@ struct JogArmShared : public std::mutex

double collision_velocity_scale = 1;

// Indicates that an incoming Cartesian command is all zero velocities
bool zero_cartesian_cmd_flag = true;
// Flag a valid incoming Cartesian command having nonzero velocities
bool have_nonzero_cartesian_cmd = false;

// Indicates that an incoming joint angle command is all zero velocities
bool zero_joint_cmd_flag = true;
// Flag a valid incoming joint angle command having nonzero velocities
bool have_nonzero_joint_cmd = false;

// Indicates that we have not received a new command in some time
bool command_is_stale = false;
Expand Down
23 changes: 14 additions & 9 deletions moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,12 +161,12 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables)
{
// Flag that incoming commands are all zero. May be used to skip calculations/publication
shared_variables.lock();
bool zero_cartesian_cmd_flag = shared_variables.zero_cartesian_cmd_flag;
bool zero_joint_cmd_flag = shared_variables.zero_joint_cmd_flag;
bool have_nonzero_cartesian_cmd = shared_variables.have_nonzero_cartesian_cmd;
henningkayser marked this conversation as resolved.
Show resolved Hide resolved
bool have_nonzero_joint_cmd = shared_variables.have_nonzero_joint_cmd;
shared_variables.unlock();

// Prioritize cartesian jogging above joint jogging
if (!zero_cartesian_cmd_flag)
if (have_nonzero_cartesian_cmd)
{
shared_variables.lock();
cartesian_deltas = shared_variables.command_deltas;
Expand All @@ -175,7 +175,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables)
if (!cartesianJogCalcs(cartesian_deltas, shared_variables))
continue;
}
else if (!zero_joint_cmd_flag)
else if (have_nonzero_joint_cmd)
{
shared_variables.lock();
joint_deltas = shared_variables.joint_command_deltas;
Expand All @@ -194,14 +194,19 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables)
bool stale_command = shared_variables.command_is_stale;
shared_variables.unlock();

if (stale_command || (zero_cartesian_cmd_flag && zero_joint_cmd_flag))
if (stale_command || (!have_nonzero_cartesian_cmd && !have_nonzero_joint_cmd))
{
suddenHalt(outgoing_command_);
zero_cartesian_cmd_flag = true;
zero_joint_cmd_flag = true;
have_nonzero_cartesian_cmd = false;
have_nonzero_joint_cmd = false;
// Reset the valid command flag so jogging stops until a new command arrives
shared_variables.lock();
shared_variables.have_nonzero_cartesian_cmd = false;
shared_variables.have_nonzero_joint_cmd = false;
shared_variables.unlock();
}

bool valid_nonzero_command = !zero_cartesian_cmd_flag || !zero_joint_cmd_flag;
bool valid_nonzero_command = have_nonzero_cartesian_cmd || have_nonzero_joint_cmd;

// Send the newest target joints
shared_variables.lock();
Expand All @@ -228,7 +233,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables)

// Store last zero-velocity message flag to prevent superfluous warnings.
// Cartesian and joint commands must both be zero.
if (zero_cartesian_cmd_flag && zero_joint_cmd_flag)
if (!have_nonzero_cartesian_cmd && !have_nonzero_joint_cmd)
{
// Avoid overflow
if (zero_velocity_count < std::numeric_limits<int>::max())
Expand Down
20 changes: 10 additions & 10 deletions moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,14 +169,14 @@ void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamp
}

// Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish
shared_variables_.zero_cartesian_cmd_flag = shared_variables_.command_deltas.twist.linear.x == 0.0 &&
shared_variables_.command_deltas.twist.linear.y == 0.0 &&
shared_variables_.command_deltas.twist.linear.z == 0.0 &&
shared_variables_.command_deltas.twist.angular.x == 0.0 &&
shared_variables_.command_deltas.twist.angular.y == 0.0 &&
shared_variables_.command_deltas.twist.angular.z == 0.0;

if (!shared_variables_.zero_cartesian_cmd_flag)
shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 ||
AndyZe marked this conversation as resolved.
Show resolved Hide resolved
shared_variables_.command_deltas.twist.linear.y != 0.0 ||
shared_variables_.command_deltas.twist.linear.z != 0.0 ||
shared_variables_.command_deltas.twist.angular.x != 0.0 ||
shared_variables_.command_deltas.twist.angular.y != 0.0 ||
shared_variables_.command_deltas.twist.angular.z != 0.0;

if (shared_variables_.have_nonzero_cartesian_cmd)
{
shared_variables_.latest_nonzero_cmd_stamp = velocity_command.header.stamp;
}
Expand All @@ -194,9 +194,9 @@ void JogCppInterface::provideJointCommand(const control_msgs::JointJog& joint_co
{
all_zeros &= (delta == 0.0);
};
shared_variables_.zero_joint_cmd_flag = all_zeros;
shared_variables_.have_nonzero_joint_cmd = !all_zeros;

if (!shared_variables_.zero_joint_cmd_flag)
if (shared_variables_.have_nonzero_joint_cmd)
{
shared_variables_.latest_nonzero_cmd_stamp = joint_command.header.stamp;
}
Expand Down
20 changes: 10 additions & 10 deletions moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -182,14 +182,14 @@ void JogROSInterface::deltaCartesianCmdCB(const geometry_msgs::TwistStampedConst
}

// Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish
shared_variables_.zero_cartesian_cmd_flag = shared_variables_.command_deltas.twist.linear.x == 0.0 &&
shared_variables_.command_deltas.twist.linear.y == 0.0 &&
shared_variables_.command_deltas.twist.linear.z == 0.0 &&
shared_variables_.command_deltas.twist.angular.x == 0.0 &&
shared_variables_.command_deltas.twist.angular.y == 0.0 &&
shared_variables_.command_deltas.twist.angular.z == 0.0;

if (!shared_variables_.zero_cartesian_cmd_flag)
shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 ||
shared_variables_.command_deltas.twist.linear.y != 0.0 ||
shared_variables_.command_deltas.twist.linear.z != 0.0 ||
shared_variables_.command_deltas.twist.angular.x != 0.0 ||
shared_variables_.command_deltas.twist.angular.y != 0.0 ||
shared_variables_.command_deltas.twist.angular.z != 0.0;

if (shared_variables_.have_nonzero_cartesian_cmd)
{
shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp;
}
Expand All @@ -208,9 +208,9 @@ void JogROSInterface::deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg)
{
all_zeros &= (delta == 0.0);
};
shared_variables_.zero_joint_cmd_flag = all_zeros;
shared_variables_.have_nonzero_joint_cmd = !all_zeros;

if (!shared_variables_.zero_joint_cmd_flag)
if (shared_variables_.have_nonzero_joint_cmd)
{
shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp;
}
Expand Down