Skip to content

Clean imarker_end_effector.cpp #151

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

Open
wants to merge 2 commits into
base: ros2
Choose a base branch
from
Open

Clean imarker_end_effector.cpp #151

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4b524bc
Clean imarker_end_effector.cpp
mosfet80 Aug 16, 2025
f191288
Update imarker_end_effector.cpp
mosfet80 Aug 16, 2025
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
38 changes: 1 addition & 37 deletions src/imarker_end_effector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,18 +59,6 @@ bool isStateValid(const planning_scene::PlanningScene* planning_scene, bool verb
robot_state->setJointGroupPositions(group, ik_solution);
robot_state->update();

#if 0 // Ensure there are objects in the planning scene
const std::size_t num_collision_objects = planning_scene->getCollisionEnv()->getWorld()->size();
if (num_collision_objects == 0)
{
RCLCPP_ERROR(LOGGER, "No collision objects exist in world, you need at least a table "
"modeled for the controller to work");
RCLCPP_ERROR(LOGGER, "To fix this, relaunch the teleop/head tracking/whatever MoveIt "
"node to publish the collision objects");
return false;
}
#endif

if (!planning_scene)
{
RCLCPP_ERROR(LOGGER, "No planning scene provided");
Expand Down Expand Up @@ -163,7 +151,6 @@ void IMarkerEndEffector::iMarkerCallback(

// Only allow one feedback to be processed at a time
{
// boost::unique_lock<boost::mutex> scoped_lock(imarker_mutex_);
if (!imarker_ready_to_process_)
{
return;
Expand All @@ -183,10 +170,7 @@ void IMarkerEndEffector::iMarkerCallback(
imarker_callback_(feedback, robot_ee_pose);

// Allow next feedback to be processed
{
// boost::unique_lock<boost::mutex> scoped_lock(imarker_mutex_);
imarker_ready_to_process_ = true;
}
imarker_ready_to_process_ = true;
}

void IMarkerEndEffector::solveIK(Eigen::Isometry3d& pose)
Expand All @@ -210,16 +194,7 @@ void IMarkerEndEffector::solveIK(Eigen::Isometry3d& pose)
if (imarker_state_->setFromIK(arm_data_.jmg_, pose, arm_data_.ee_link_->getName(), timeout, constraint_fn))
{
imarker_state_->update();
// if (psm_->getPlanningScene()->isStateValid(*imarker_state_))
//{
// ROS_INFO_STREAM_NAMED(name_, "Solved IK");
imarker_parent_->publishRobotState();
//}
// else
// {
// visual_tools_->publishRobotState(imarker_state_, rviz_visual_tools::RED);
// exit(0);
// }
}
}

Expand All @@ -235,7 +210,6 @@ void IMarkerEndEffector::initializeInteractiveMarkers()
void IMarkerEndEffector::updateIMarkerPose(const Eigen::Isometry3d& /*pose*/)
{
// Move marker to tip of fingers
// imarker_pose_ = pose * imarker_offset_.inverse();
sendUpdatedIMarkerPose();
}

Expand All @@ -255,15 +229,7 @@ void IMarkerEndEffector::make6DofMarker(const geometry_msgs::msg::Pose& pose)
int_marker_.header.frame_id = "world";
int_marker_.pose = pose;
int_marker_.scale = 0.2;

int_marker_.name = name_;
// int_marker_.description = "imarker_" + name_; // TODO: unsure, but I think this causes a caption in Rviz that I
// don't want

// insert a box
// makeBoxControl(int_marker_);

// int_marker_.controls[0].interaction_mode = InteractiveMarkerControl::MENU;

InteractiveMarkerControl control;
control.orientation.w = 1;
Expand Down Expand Up @@ -300,8 +266,6 @@ void IMarkerEndEffector::make6DofMarker(const geometry_msgs::msg::Pose& pose)
int_marker_.controls.push_back(control);

imarker_server_->insert(int_marker_, std::bind(&IMarkerEndEffector::iMarkerCallback, this, std::placeholders::_1));

// menu_handler_.apply(*imarker_server_, int_marker_.name);
}

visualization_msgs::msg::InteractiveMarkerControl&
Expand Down
Loading