Skip to content

Commit 5ab1e72

Browse files
davetcolemandavetcoleman
authored
Switch to LOGNAME style re:moveit (#42)
1 parent 5c7deaa commit 5ab1e72

File tree

2 files changed

+47
-43
lines changed

2 files changed

+47
-43
lines changed

‎src/imarker_robot_state.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -272,7 +272,8 @@ bool IMarkerRobotState::getFilePath(std::string& file_path, const std::string& f
272272
return true;
273273
}
274274

275-
bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d poses, const moveit::core::JointModelGroup* group)
275+
bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d poses,
276+
const moveit::core::JointModelGroup* group)
276277
{
277278
std::vector<std::string> tips;
278279
for (std::size_t i = 0; i < arm_datas_.size(); ++i)

‎src/moveit_visual_tools.cpp‎

Lines changed: 45 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,8 @@
6666

6767
namespace moveit_visual_tools
6868
{
69+
const std::string LOGNAME = "visual_tools";
70+
6971
MoveItVisualTools::MoveItVisualTools(const std::string& base_frame, const std::string& marker_topic,
7072
planning_scene_monitor::PlanningSceneMonitorPtr psm)
7173
: RvizVisualTools::RvizVisualTools(base_frame, marker_topic)
@@ -86,11 +88,11 @@ bool MoveItVisualTools::loadPlanningSceneMonitor()
8688
// Check if we already have one
8789
if (psm_)
8890
{
89-
ROS_WARN_STREAM_NAMED(name_, "Will not load a new planning scene monitor when one has "
90-
"already been set for Visual Tools");
91+
ROS_WARN_STREAM_NAMED(LOGNAME, "Will not load a new planning scene monitor when one has "
92+
"already been set for Visual Tools");
9193
return false;
9294
}
93-
ROS_DEBUG_STREAM_NAMED(name_, "Loading planning scene monitor");
95+
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Loading planning scene monitor");
9496

9597
// Create tf transform buffer and listener
9698
std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::make_shared<tf2_ros::Buffer>();
@@ -112,14 +114,14 @@ bool MoveItVisualTools::loadPlanningSceneMonitor()
112114

113115
psm_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
114116
planning_scene_topic_);
115-
ROS_DEBUG_STREAM_NAMED(name_, "Publishing planning scene on " << planning_scene_topic_);
117+
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Publishing planning scene on " << planning_scene_topic_);
116118

117119
planning_scene_monitor::LockedPlanningSceneRW planning_scene(psm_);
118120
planning_scene->setName("visual_tools_scene");
119121
}
120122
else
121123
{
122-
ROS_ERROR_STREAM_NAMED(name_, "Planning scene not configured");
124+
ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured");
123125
return false;
124126
}
125127

@@ -181,8 +183,8 @@ bool MoveItVisualTools::moveCollisionObject(const geometry_msgs::Pose& pose, con
181183
collision_obj.primitive_poses.resize(1);
182184
collision_obj.primitive_poses[0] = pose;
183185

184-
// ROS_INFO_STREAM_NAMED(name_,"CollisionObject: \n " << collision_obj);
185-
// ROS_DEBUG_STREAM_NAMED(name_,"Published collision object " << name);
186+
// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
187+
// ROS_DEBUG_STREAM_NAMED(LOGNAME,"Published collision object " << name);
186188
return processCollisionObjectMsg(collision_obj, color);
187189
}
188190

@@ -242,7 +244,7 @@ bool MoveItVisualTools::loadEEMarker(const robot_model::JointModelGroup* ee_jmg,
242244
// Get joint state group
243245
if (ee_jmg == NULL) // make sure EE_GROUP exists
244246
{
245-
ROS_ERROR_STREAM_NAMED(name_, "Unable to find joint model group with address" << ee_jmg);
247+
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to find joint model group with address" << ee_jmg);
246248
return false;
247249
}
248250

@@ -255,9 +257,9 @@ bool MoveItVisualTools::loadEEMarker(const robot_model::JointModelGroup* ee_jmg,
255257
{
256258
if (ee_joint_pos.size() != ee_jmg->getActiveJointModels().size())
257259
{
258-
ROS_ERROR_STREAM_NAMED(name_, "The number of joint positions given ("
259-
<< ee_joint_pos.size() << ") does not match the number of active joints in "
260-
<< ee_jmg->getName() << "(" << ee_jmg->getActiveJointModels().size() << ")");
260+
ROS_ERROR_STREAM_NAMED(LOGNAME, "The number of joint positions given ("
261+
<< ee_joint_pos.size() << ") does not match the number of active joints in "
262+
<< ee_jmg->getName() << "(" << ee_jmg->getActiveJointModels().size() << ")");
261263
return false;
262264
}
263265
shared_robot_state_->setJointGroupPositions(ee_jmg, ee_joint_pos);
@@ -288,10 +290,10 @@ bool MoveItVisualTools::loadEEMarker(const robot_model::JointModelGroup* ee_jmg,
288290

289291
shared_robot_state_->getRobotMarkers(ee_markers_map_[ee_jmg], ee_link_names, marker_color, ee_jmg->getName(),
290292
ros::Duration());
291-
ROS_DEBUG_STREAM_NAMED(name_, "Number of rviz markers in end effector: " << ee_markers_map_[ee_jmg].markers.size());
293+
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Number of rviz markers in end effector: " << ee_markers_map_[ee_jmg].markers.size());
292294

293295
const std::string& ee_parent_link_name = ee_jmg->getEndEffectorParentGroup().second;
294-
// ROS_DEBUG_STREAM_NAMED(name_,"EE Parent link: " << ee_parent_link_name);
296+
// ROS_DEBUG_STREAM_NAMED(LOGNAME,"EE Parent link: " << ee_parent_link_name);
295297
const moveit::core::LinkModel* ee_parent_link = robot_model_->getLinkModel(ee_parent_link_name);
296298

297299
Eigen::Isometry3d ee_marker_global_transform = shared_robot_state_->getGlobalLinkTransform(ee_parent_link);
@@ -329,7 +331,7 @@ void MoveItVisualTools::loadTrajectoryPub(const std::string& display_planned_pat
329331

330332
// Trajectory paths
331333
pub_display_path_ = nh_.advertise<moveit_msgs::DisplayTrajectory>(display_planned_path_topic, 10, false);
332-
ROS_DEBUG_STREAM_NAMED(name_, "Publishing MoveIt! trajectory on topic " << pub_display_path_.getTopic());
334+
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Publishing MoveIt! trajectory on topic " << pub_display_path_.getTopic());
333335

334336
// Wait for topic to be ready
335337
if (blocking)
@@ -347,7 +349,7 @@ void MoveItVisualTools::loadRobotStatePub(const std::string& robot_state_topic,
347349

348350
// RobotState Message
349351
pub_robot_state_ = nh_.advertise<moveit_msgs::DisplayRobotState>(robot_state_topic_, 1);
350-
ROS_DEBUG_STREAM_NAMED(name_, "Publishing MoveIt! robot state on topic " << pub_robot_state_.getTopic());
352+
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Publishing MoveIt! robot state on topic " << pub_robot_state_.getTopic());
351353

352354
// Wait for topic to be ready
353355
if (blocking)
@@ -364,7 +366,7 @@ bool MoveItVisualTools::publishEEMarkers(const geometry_msgs::Pose& pose, const
364366
{
365367
if (!loadEEMarker(ee_jmg, ee_joint_pos))
366368
{
367-
ROS_ERROR_STREAM_NAMED(name_, "Unable to publish EE marker, unable to load EE markers");
369+
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to publish EE marker, unable to load EE markers");
368370
return false;
369371
}
370372
}
@@ -409,8 +411,8 @@ bool MoveItVisualTools::publishEEMarkers(const geometry_msgs::Pose& pose, const
409411
bool MoveItVisualTools::publishGrasps(const std::vector<moveit_msgs::Grasp>& possible_grasps,
410412
const robot_model::JointModelGroup* ee_jmg, double animate_speed)
411413
{
412-
ROS_DEBUG_STREAM_NAMED(name_, "Visualizing " << possible_grasps.size() << " grasps with EE joint model group "
413-
<< ee_jmg->getName());
414+
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Visualizing " << possible_grasps.size() << " grasps with EE joint model group "
415+
<< ee_jmg->getName());
414416

415417
// Loop through all grasps
416418
for (std::size_t i = 0; i < possible_grasps.size(); ++i)
@@ -429,8 +431,8 @@ bool MoveItVisualTools::publishGrasps(const std::vector<moveit_msgs::Grasp>& pos
429431
bool MoveItVisualTools::publishAnimatedGrasps(const std::vector<moveit_msgs::Grasp>& possible_grasps,
430432
const robot_model::JointModelGroup* ee_jmg, double animate_speed)
431433
{
432-
ROS_DEBUG_STREAM_NAMED(name_, "Visualizing " << possible_grasps.size() << " grasps with joint model group "
433-
<< ee_jmg->getName() << " at speed " << animate_speed);
434+
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Visualizing " << possible_grasps.size() << " grasps with joint model group "
435+
<< ee_jmg->getName() << " at speed " << animate_speed);
434436

435437
// Loop through all grasps
436438
for (std::size_t i = 0; i < possible_grasps.size(); ++i)
@@ -533,13 +535,13 @@ bool MoveItVisualTools::publishIKSolutions(const std::vector<trajectory_msgs::Jo
533535
{
534536
if (ik_solutions.empty())
535537
{
536-
ROS_WARN_STREAM_NAMED(name_, "Empty ik_solutions vector passed into publishIKSolutions()");
538+
ROS_WARN_STREAM_NAMED(LOGNAME, "Empty ik_solutions vector passed into publishIKSolutions()");
537539
return false;
538540
}
539541

540542
loadSharedRobotState();
541543

542-
ROS_DEBUG_STREAM_NAMED(name_, "Visualizing " << ik_solutions.size() << " inverse kinematic solutions");
544+
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Visualizing " << ik_solutions.size() << " inverse kinematic solutions");
543545

544546
// Apply the time to the trajectory
545547
trajectory_msgs::JointTrajectoryPoint trajectory_pt_timed;
@@ -641,8 +643,8 @@ bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_p
641643
collision_obj.primitive_poses.resize(1);
642644
collision_obj.primitive_poses[0] = block_pose;
643645

644-
// ROS_INFO_STREAM_NAMED(name_,"CollisionObject: \n " << collision_obj);
645-
// ROS_DEBUG_STREAM_NAMED(name_,"Published collision object " << name);
646+
// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
647+
// ROS_DEBUG_STREAM_NAMED(LOGNAME,"Published collision object " << name);
646648
return processCollisionObjectMsg(collision_obj, color);
647649
}
648650

@@ -684,7 +686,7 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point
684686
if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z])
685687
collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = rviz_visual_tools::SMALL_SCALE;
686688

687-
// ROS_INFO_STREAM_NAMED(name_,"CollisionObject: \n " << collision_obj);
689+
// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
688690
return processCollisionObjectMsg(collision_obj, color);
689691
}
690692

@@ -799,7 +801,7 @@ bool MoveItVisualTools::publishCollisionCylinder(const geometry_msgs::Pose& obje
799801
collision_obj.primitive_poses.resize(1);
800802
collision_obj.primitive_poses[0] = object_pose;
801803

802-
// ROS_INFO_STREAM_NAMED(name_,"CollisionObject: \n " << collision_obj);
804+
// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
803805
return processCollisionObjectMsg(collision_obj, color);
804806
}
805807

@@ -816,14 +818,14 @@ bool MoveItVisualTools::publishCollisionMesh(const geometry_msgs::Pose& object_p
816818
shapes::ShapeMsg shape_msg; // this is a boost::variant type from shape_messages.h
817819
if (!mesh || !shapes::constructMsgFromShape(mesh, shape_msg))
818820
{
819-
ROS_ERROR_STREAM_NAMED(name_, "Unable to create mesh shape message from resource " << mesh_path);
821+
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to create mesh shape message from resource " << mesh_path);
820822
return false;
821823
}
822824

823825
if (!publishCollisionMesh(object_pose, object_name, boost::get<shape_msgs::Mesh>(shape_msg), color))
824826
return false;
825827

826-
ROS_DEBUG_NAMED(name_, "Loaded mesh from '%s'", mesh_path.c_str());
828+
ROS_DEBUG_NAMED(LOGNAME, "Loaded mesh from '%s'", mesh_path.c_str());
827829
return true;
828830
}
829831

@@ -1035,14 +1037,14 @@ bool MoveItVisualTools::loadCollisionSceneFromFile(const std::string& path, cons
10351037
}
10361038
else
10371039
{
1038-
ROS_WARN_STREAM_NAMED(name_, "Unable to get locked planning scene RW");
1040+
ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to get locked planning scene RW");
10391041
return false;
10401042
}
10411043
}
1042-
ROS_INFO_NAMED(name_, "Loaded scene geometry from '%s'", path.c_str());
1044+
ROS_INFO_NAMED(LOGNAME, "Loaded scene geometry from '%s'", path.c_str());
10431045
}
10441046
else
1045-
ROS_WARN_NAMED(name_, "Unable to load scene geometry from '%s'", path.c_str());
1047+
ROS_WARN_NAMED(LOGNAME, "Unable to load scene geometry from '%s'", path.c_str());
10461048

10471049
fin.close();
10481050

@@ -1075,7 +1077,7 @@ bool MoveItVisualTools::publishContactPoints(const moveit::core::RobotState& rob
10751077
{
10761078
visualization_msgs::MarkerArray arr;
10771079
collision_detection::getCollisionMarkersFromContacts(arr, planning_scene->getPlanningFrame(), c_res.contacts);
1078-
ROS_INFO_STREAM_NAMED(name_, "Completed listing of explanations for invalid states.");
1080+
ROS_INFO_STREAM_NAMED(LOGNAME, "Completed listing of explanations for invalid states.");
10791081

10801082
// Check for markers
10811083
if (arr.markers.empty())
@@ -1189,7 +1191,7 @@ bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory
11891191
// Check if we have enough points
11901192
if (!trajectory_msg.joint_trajectory.points.size())
11911193
{
1192-
ROS_WARN_STREAM_NAMED(name_, "Unable to publish trajectory path because trajectory has zero points");
1194+
ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to publish trajectory path because trajectory has zero points");
11931195
return false;
11941196
}
11951197

@@ -1215,7 +1217,7 @@ bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory
12151217
{
12161218
duration = 0.05 * trajectory_msg.joint_trajectory.points.size();
12171219
}
1218-
ROS_DEBUG_STREAM_NAMED(name_, "Waiting for trajectory animation " << duration << " seconds");
1220+
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for trajectory animation " << duration << " seconds");
12191221

12201222
// Check if ROS is ok in intervals
12211223
double counter = 0;
@@ -1238,7 +1240,7 @@ bool MoveItVisualTools::publishTrajectoryLine(const moveit_msgs::RobotTrajectory
12381240
// Error check
12391241
if (!arm_jmg)
12401242
{
1241-
ROS_FATAL_STREAM_NAMED(name_, "arm_jmg is NULL");
1243+
ROS_FATAL_STREAM_NAMED(LOGNAME, "arm_jmg is NULL");
12421244
return false;
12431245
}
12441246

@@ -1267,7 +1269,7 @@ bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTraje
12671269
// Error check
12681270
if (!ee_parent_link)
12691271
{
1270-
ROS_FATAL_STREAM_NAMED(name_, "ee_parent_link is NULL");
1272+
ROS_FATAL_STREAM_NAMED(LOGNAME, "ee_parent_link is NULL");
12711273
return false;
12721274
}
12731275

@@ -1282,7 +1284,7 @@ bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTraje
12821284
// Error Check
12831285
if (tip_pose.translation().x() != tip_pose.translation().x())
12841286
{
1285-
ROS_ERROR_STREAM_NAMED(name_, "NAN DETECTED AT TRAJECTORY POINT i=" << i);
1287+
ROS_ERROR_STREAM_NAMED(LOGNAME, "NAN DETECTED AT TRAJECTORY POINT i=" << i);
12861288
return false;
12871289
}
12881290

@@ -1303,7 +1305,7 @@ bool MoveItVisualTools::publishTrajectoryLine(const moveit_msgs::RobotTrajectory
13031305
std::vector<const moveit::core::LinkModel*> tips;
13041306
if (!arm_jmg->getEndEffectorTips(tips))
13051307
{
1306-
ROS_ERROR_STREAM_NAMED(name_, "Unable to get end effector tips from jmg");
1308+
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to get end effector tips from jmg");
13071309
return false;
13081310
}
13091311

@@ -1331,7 +1333,7 @@ bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTraje
13311333
std::vector<const moveit::core::LinkModel*> tips;
13321334
if (!arm_jmg->getEndEffectorTips(tips))
13331335
{
1334-
ROS_ERROR_STREAM_NAMED(name_, "Unable to get end effector tips from jmg");
1336+
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unable to get end effector tips from jmg");
13351337
return false;
13361338
}
13371339

@@ -1482,7 +1484,7 @@ void MoveItVisualTools::showJointLimits(robot_state::RobotStatePtr robot_state)
14821484
// Assume all joints have only one variable
14831485
if (joints[i]->getVariableCount() > 1)
14841486
{
1485-
// ROS_WARN_STREAM_NAMED(name_, "Unable to handle joints with more than one var, skipping '"
1487+
// ROS_WARN_STREAM_NAMED(LOGNAME, "Unable to handle joints with more than one var, skipping '"
14861488
//<< joints[i]->getName() << "'");
14871489
continue;
14881490
}
@@ -1533,7 +1535,7 @@ planning_scene_monitor::PlanningSceneMonitorPtr MoveItVisualTools::getPlanningSc
15331535
{
15341536
if (!psm_)
15351537
{
1536-
ROS_INFO_STREAM_NAMED(name_, "No planning scene passed into moveit_visual_tools, creating one.");
1538+
ROS_INFO_STREAM_NAMED(LOGNAME, "No planning scene passed into moveit_visual_tools, creating one.");
15371539
loadPlanningSceneMonitor();
15381540
ros::spinOnce();
15391541
ros::Duration(1).sleep(); // TODO: is this necessary?
@@ -1573,7 +1575,8 @@ bool MoveItVisualTools::checkForVirtualJoint(const moveit::core::RobotState& rob
15731575
return true;
15741576
}
15751577

1576-
bool MoveItVisualTools::applyVirtualJointTransform(moveit::core::RobotState& robot_state, const Eigen::Isometry3d& offset)
1578+
bool MoveItVisualTools::applyVirtualJointTransform(moveit::core::RobotState& robot_state,
1579+
const Eigen::Isometry3d& offset)
15771580
{
15781581
static const std::string VJOINT_NAME = "virtual_joint";
15791582

0 commit comments

Comments
 (0)