Skip to content
This repository has been archived by the owner on Nov 13, 2017. It is now read-only.

Allowed planning scene monitor to ignore joint states that are not used #308

Closed
wants to merge 1 commit into from
Closed

Allowed planning scene monitor to ignore joint states that are not used #308

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,4 +46,7 @@ qtcreator-*
*~$

# Emacs
.#*
.#*

# Catkin custom files
CATKIN_IGNORE
18 changes: 13 additions & 5 deletions planning/planning_scene_monitor/src/current_state_monitor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -308,22 +308,30 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso
current_state_time_ = joint_state->header.stamp;
for (std::size_t i = 0 ; i < n ; ++i)
{
robot_state_.setVariablePosition(joint_state->name[i], joint_state->position[i]);
// check if this joint name is non-fixed in our urdf
int joint_index;
if( !robot_model_->getVariableIndex(joint_state->name[i], joint_index) )
{
ROS_WARN_STREAM_ONCE("Joint '" << joint_state->name[i] << "' was published in the joint state message but is not a valid joint variable. Ignoring.");
continue;
}

robot_state_.setVariablePosition(joint_index, joint_state->position[i]);
joint_time_[joint_state->name[i]] = joint_state->header.stamp;

// continuous joints wrap, so we don't modify them (even if they are outside bounds!)
const robot_model::JointModel* jm = robot_model_->getJointModel(joint_state->name[i]);
const robot_model::JointModel* jm = robot_model_->getJointModel(joint_index);
if (jm && jm->getType() == robot_model::JointModel::REVOLUTE)
if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
continue;

const robot_model::VariableBounds &b = robot_model_->getVariableBounds(joint_state->name[i]);
const robot_model::VariableBounds &b = robot_model_->getVariableBounds(joint_state->name[i]); // \todo make this depend on joint index
// if the read variable is 'almost' within bounds (up to error_ difference), then consider it to be within bounds
if (joint_state->position[i] < b.min_position_ && joint_state->position[i] >= b.min_position_ - error_)
robot_state_.setVariablePosition(joint_state->name[i], b.min_position_);
robot_state_.setVariablePosition(joint_state->name[i], b.min_position_); // \todo make this depend on joint index
else
if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ + error_)
robot_state_.setVariablePosition(joint_state->name[i], b.max_position_);
robot_state_.setVariablePosition(joint_state->name[i], b.max_position_); // \todo make this depend on joint index
}

// read root transform, if needed
Expand Down