Skip to content

Commit

Permalink
Clean up
Browse files Browse the repository at this point in the history 
Update loggings


Apply clang format
  • Loading branch information
@cambel
cambel committed Jul 4, 2022
1 parent dc41295 commit 04cec9b
Show file tree
Hide file tree
Showing 6 changed files with 170 additions and 217 deletions.
31 changes: 16 additions & 15 deletions moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ BaseFakeController::BaseFakeController(const std::string& name, const std::vecto
ss << "Fake controller '" << name << "' with joints [ ";
std::copy(joints.begin(), joints.end(), std::ostream_iterator<std::string>(ss, " "));
ss << "]";
ROS_INFO_STREAM_NAMED(name_, ss.str());
ROS_INFO_STREAM_NAMED(LOGNAME, ss.str());
}

void BaseFakeController::getJoints(std::vector<std::string>& joints) const
Expand All @@ -74,7 +74,7 @@ LastPointController::~LastPointController() = default;

bool LastPointController::sendTrajectory(const moveit_msgs::RobotTrajectory& t)
{
ROS_INFO_NAMED(name_, "Fake execution of trajectory: Last point start");
ROS_INFO_NAMED(LOGNAME, "Fake execution of trajectory: Last point start");
if (t.joint_trajectory.points.empty())
return true;

Expand All @@ -89,7 +89,7 @@ bool LastPointController::sendTrajectory(const moveit_msgs::RobotTrajectory& t)
js.effort = last.effort;
pub_.publish(js);
status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
ROS_INFO_NAMED(name_, "Fake execution of trajectory: Last point done");
ROS_INFO_NAMED(LOGNAME, "Fake execution of trajectory: Last point done");

return true;
}
Expand Down Expand Up @@ -135,7 +135,7 @@ bool ThreadedController::sendTrajectory(const moveit_msgs::RobotTrajectory& t)
bool ThreadedController::cancelExecution()
{
cancelTrajectory();
ROS_INFO_NAMED(name_, "Fake trajectory execution cancelled");
ROS_INFO_NAMED(LOGNAME, "Fake trajectory execution cancelled");
status_ = moveit_controller_manager::ExecutionStatus::ABORTED;
return true;
}
Expand All @@ -162,7 +162,7 @@ ViaPointController::~ViaPointController() = default;

void ViaPointController::execTrajectory(const moveit_msgs::RobotTrajectory& t)
{
ROS_INFO_NAMED(name_, "Fake execution of trajectory: start");
ROS_INFO_NAMED(LOGNAME, "Fake execution of trajectory: start");
status_ = moveit_controller_manager::ExecutionStatus::RUNNING;
sensor_msgs::JointState js;
js.header = t.joint_trajectory.header;
Expand All @@ -182,14 +182,15 @@ void ViaPointController::execTrajectory(const moveit_msgs::RobotTrajectory& t)
ros::Duration wait_time = via->time_from_start - (ros::Time::now() - start_time);
if (wait_time.toSec() > std::numeric_limits<float>::epsilon())
{
ROS_DEBUG_NAMED(name_, "Fake execution: waiting %0.1fs for next via point, %ld remaining", wait_time.toSec(), end - via);
ROS_DEBUG_NAMED(LOGNAME, "Fake execution: waiting %0.1fs for next via point, %ld remaining", wait_time.toSec(),
end - via);
wait_time.sleep();
}
js.header.stamp = ros::Time::now();
pub_.publish(js);
}
status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
ROS_INFO_NAMED(name_, "Fake execution of trajectory: done");
ROS_INFO_NAMED(LOGNAME, "Fake execution of trajectory: done");
}

InterpolatingController::InterpolatingController(const std::string& name, const std::vector<std::string>& joints,
Expand Down Expand Up @@ -223,7 +224,7 @@ void interpolate(sensor_msgs::JointState& js, const trajectory_msgs::JointTrajec

void InterpolatingController::execTrajectory(const moveit_msgs::RobotTrajectory& t)
{
ROS_INFO_NAMED(name_, "Fake execution of trajectory: start");
ROS_INFO_NAMED(LOGNAME, "Fake execution of trajectory: start");
status_ = moveit_controller_manager::ExecutionStatus::RUNNING;
if (t.joint_trajectory.points.empty())
return;
Expand Down Expand Up @@ -251,10 +252,10 @@ void InterpolatingController::execTrajectory(const moveit_msgs::RobotTrajectory&
break;

double duration = (next->time_from_start - prev->time_from_start).toSec();
ROS_DEBUG_NAMED(name_, "elapsed: %.3f via points %td,%td / %td alpha: %.3f", elapsed.toSec(), prev - points.begin(),
next - points.begin(), end - points.begin(),
duration > std::numeric_limits<double>::epsilon() ? (elapsed - prev->time_from_start).toSec() / duration :
1.0);
ROS_DEBUG_NAMED(
LOGNAME, "elapsed: %.3f via points %td,%td / %td alpha: %.3f", elapsed.toSec(), prev - points.begin(),
next - points.begin(), end - points.begin(),
duration > std::numeric_limits<double>::epsilon() ? (elapsed - prev->time_from_start).toSec() / duration : 1.0);
interpolate(js, *prev, *next, elapsed);
js.header.stamp = ros::Time::now();
pub_.publish(js);
Expand All @@ -267,15 +268,15 @@ void InterpolatingController::execTrajectory(const moveit_msgs::RobotTrajectory&
}

ros::Duration elapsed = ros::Time::now() - start_time;
ROS_DEBUG_NAMED(name_, "elapsed: %.3f via points %td,%td / %td alpha: 1.0", elapsed.toSec(), prev - points.begin(),
next - points.begin(), end - points.begin());
ROS_DEBUG_NAMED(LOGNAME, "elapsed: %.3f via points %td,%td / %td alpha: 1.0", elapsed.toSec(), prev - points.begin(),
next - points.begin(), end - points.begin());

// publish last point
interpolate(js, *prev, *prev, prev->time_from_start);
js.header.stamp = ros::Time::now();
pub_.publish(js);
status_ = moveit_controller_manager::ExecutionStatus::SUCCEEDED;
ROS_INFO_NAMED(name_, "Fake execution of trajectory: done");
ROS_INFO_NAMED(LOGNAME, "Fake execution of trajectory: done");
}

} // end namespace moveit_fake_controller_manager
3 changes: 2 additions & 1 deletion moveit_plugins/moveit_fake_controller_manager/src/moveit_fake_controllers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@

namespace moveit_fake_controller_manager
{
const std::string LOGNAME = "planning_scene";

MOVEIT_CLASS_FORWARD(BaseFakeController); // Defines BaseFakeControllerPtr, ConstPtr, WeakPtr... etc

// common base class to all fake controllers in this package
Expand All @@ -61,7 +63,6 @@ class BaseFakeController : public moveit_controller_manager::MoveItControllerHan
std::vector<std::string> joints_;
const ros::Publisher& pub_;
moveit_controller_manager::ExecutionStatus status_;
std::string name_ = "fake_controllers";
};

class LastPointController : public BaseFakeController
Expand Down
22 changes: 10 additions & 12 deletions moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,8 @@ void MoveGroupExecuteTrajectoryAction::initialize()

void MoveGroupExecuteTrajectoryAction::executePathCallback(ExecuteTrajectoryActionServer::GoalHandle goal)
{
ROS_INFO_NAMED(name_, "Goal received (ExecuteTrajectoryActionServer)");
ROS_DEBUG_STREAM_NAMED(name_, "Goal ID" << goal.getGoalID());
ROS_INFO_NAMED(LOGNAME, "Goal received (ExecuteTrajectoryActionServer)");
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Goal ID" << goal.getGoalID());
moveit_msgs::ExecuteTrajectoryGoal goal_msg = *goal.getGoal();
moveit_msgs::ExecuteTrajectoryResult action_res;

Expand All @@ -74,12 +74,10 @@ void MoveGroupExecuteTrajectoryAction::executePathCallback(ExecuteTrajectoryActi

// Define a lambda that sets the goal state when the trajectory is completed.
// This is called when the trajectory finishes.
bool execution_complete = false;
auto goalId = goal.getGoalID();
// Copy goalId here so that it is still available when this lambda is called later (otherwise it is deleted when the parent function ends)
auto completedTrajectoryCallback = [&, goalId](const moveit_controller_manager::ExecutionStatus& status) {
ROS_DEBUG_NAMED(name_, "Entered lambda");
execution_complete = true;
ROS_DEBUG_NAMED(LOGNAME, "Entered lambda");
if (status == moveit_controller_manager::ExecutionStatus::SUCCEEDED)
{
action_res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
Expand All @@ -96,20 +94,20 @@ void MoveGroupExecuteTrajectoryAction::executePathCallback(ExecuteTrajectoryActi
{
action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
}
ROS_DEBUG_STREAM_NAMED(name_, "Execution completed: " << status.asString());
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Execution completed: " << status.asString());

ROS_DEBUG_NAMED(name_, "getting ActionResultString");
ROS_DEBUG_NAMED(LOGNAME, "getting ActionResultString");
const std::string response = this->getActionResultString(action_res.error_code, false, false);

ROS_DEBUG_STREAM_NAMED(name_, "List of goal_handles_: " << goal_handles_.size());
ROS_DEBUG_STREAM_NAMED(LOGNAME, "List of goal_handles_: " << goal_handles_.size());

{
boost::mutex::scoped_lock lock(goal_handles_mutex_);
for (auto it = goal_handles_.begin(); it != goal_handles_.end(); ++it)
{
if (it->getGoalID() == goalId)
{
ROS_DEBUG_STREAM_NAMED(name_, "goal (" << it->getGoal()->trajectory.joint_trajectory.joint_names[0] << ") set to " << response);
ROS_DEBUG_STREAM_NAMED(LOGNAME, "goal (" << it->getGoal()->trajectory.joint_trajectory.joint_names[0] << ") set to " << response);
if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
{
it->setSucceeded(action_res, response);
Expand All @@ -122,20 +120,20 @@ void MoveGroupExecuteTrajectoryAction::executePathCallback(ExecuteTrajectoryActi
break;
}
}
// ROS_DEBUG_STREAM_NAMED(name_, "goal set to " << response);
// ROS_DEBUG_STREAM_NAMED(LOGNAME, "goal set to " << response);
}
}; // end of lambda expression

{
boost::mutex::scoped_lock lock(goal_handles_mutex_);
if (context_->trajectory_execution_manager_->pushAndExecuteSimultaneous(goal.getGoal()->trajectory, "", completedTrajectoryCallback))
{
ROS_DEBUG_STREAM_NAMED(name_, "Pushed trajectory to queue.");
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Pushed trajectory to queue.");
goal_handles_.push_back(goal);
}
else
{
ROS_DEBUG_STREAM_NAMED(name_, "Failed to pushed trajectory to queue.");
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Failed to push trajectory to queue.");
action_res.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
const std::string response = getActionResultString(action_res.error_code, false, false);
goal.setAborted(action_res, response);
Expand Down
4 changes: 3 additions & 1 deletion moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
typedef actionlib::ActionServer<moveit_msgs::ExecuteTrajectoryAction> ExecuteTrajectoryActionServer;
namespace move_group
{
const std::string LOGNAME = "trajectory_execution_action_capability";

class MoveGroupExecuteTrajectoryAction : public MoveGroupCapability
{
Expand All @@ -59,8 +60,9 @@ class MoveGroupExecuteTrajectoryAction : public MoveGroupCapability
private:
void executePathCallback(ExecuteTrajectoryActionServer::GoalHandle goal);
void setExecuteTrajectoryState(MoveGroupState state);
// TODO(cambel): Update preempt logic with cancelling goals
// void preemptExecuteTrajectoryCallback();

const std::string name_ = "trajectory_execution_action_capability";
boost::mutex goal_handles_mutex_;

std::unique_ptr<ExecuteTrajectoryActionServer> execute_action_server_;
Expand Down
5 changes: 2 additions & 3 deletions ...cution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,8 @@

namespace trajectory_execution_manager
{
const std::string LOGNAME = "trajectory_execution_manager";

MOVEIT_CLASS_FORWARD(TrajectoryExecutionManager); // Defines TrajectoryExecutionManagerPtr, ConstPtr, WeakPtr... etc

// Two modes:
Expand Down Expand Up @@ -337,9 +339,6 @@ class TrajectoryExecutionManager

bool checkCollisionsWithCurrentState(moveit_msgs::RobotTrajectory& trajectory);

// Name of this class for logging
const std::string name_ = "trajectory_execution_manager";

moveit::core::RobotModelConstPtr robot_model_;
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
planning_scene_monitor::CurrentStateMonitorPtr csm_;
Expand Down
Loading

0 comments on commit 04cec9b

Please sign in to comment.