CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

clever.launch: change log formatting (#175)

Browse Source 
* clever.launch: Add node name to output format

* selfcheck: Use new log format

* aruco_pose, clever: Remove node names from messages

* aruco_pose, clever: Use nodelet-aware rosconsole macros

* clever.launch: Use logger name instead of node name

* clever.launch: change rosconsole format a little
This commit is contained in:
Alexey Rogachevskiy
2019-09-06 17:47:31 +03:00
committed by Oleg Kalachev
parent fb5f8e5078
commit 50739a3ea8
7 changed files with 41 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
aruco_pose/src/aruco_detect.cpp
View File

@@ -84,7 +84,7 @@ public:
nh_priv_.param("estimate_poses", estimate_poses_, true);
nh_priv_.param("send_tf", send_tf_, true);
if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined");
NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
ros::shutdown();
}
readLengthOverride();
@@ -110,7 +110,7 @@ public:
vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
ROS_INFO("aruco_detect: ready");
NODELET_INFO("ready");
}
private:
@@ -162,7 +162,7 @@ private:
snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_,
msg->header.stamp, ros::Duration(0.02));
} catch (const tf2::TransformException& e) {
ROS_WARN_THROTTLE(5, "aruco_detect: can't snap: %s", e.what());
NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
}
}
}

38
aruco_pose/src/aruco_map.cpp
View File

@@ -118,7 +118,7 @@ public:
} else if (type == "gridboard") {
createGridBoard();
} else {
ROS_FATAL("aruco_map: unknown type: %s", type.c_str());
NODELET_FATAL("unknown type: %s", type.c_str());
ros::shutdown();
}
@@ -138,7 +138,7 @@ public:
publishMapImage();
vis_markers_pub_.publish(vis_array_);
ROS_INFO("aruco_map: ready");
NODELET_INFO("ready");
}
void callback(const sensor_msgs::ImageConstPtr& image,
@@ -198,7 +198,7 @@ public:
known_tilt_, markers->header.stamp, ros::Duration(0.02));
snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
} catch (const tf2::TransformException& e) {
ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what());
NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
}
geometry_msgs::TransformStamped shift;
@@ -271,7 +271,7 @@ publish_debug:
std::string line;
if (!f.good()) {
ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str());
NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
ros::shutdown();
}
@@ -289,7 +289,7 @@ publish_debug:
}
if (first == '#') {
ROS_DEBUG("aruco_map: Skipping line as a comment: %s", line.c_str());
NODELET_DEBUG("Skipping line as a comment: %s", line.c_str());
continue;
} else if (isdigit(first)) {
// Put the digit back into the stream
@@ -298,43 +298,43 @@ publish_debug:
s.putback(first);
} else {
// Probably garbage data; inform user and throw an exception, possibly killing nodelet
ROS_FATAL("aruco_map: Malformed input: %s", line.c_str());
NODELET_FATAL("Malformed input: %s", line.c_str());
ros::shutdown();
throw std::runtime_error("Malformed input");
}
if (!(s >> id >> length >> x >> y)) {
ROS_ERROR("aruco_map: Not enough data in line: %s; "
NODELET_ERROR("Not enough data in line: %s; "
"Each marker must have at least id, length, x, y fields", line.c_str());
continue;
}
// Be less strict about z, yaw, pitch roll
if (!(s >> z)) {
ROS_DEBUG("aruco_map: No z coordinate provided for marker %d, assuming 0", id);
NODELET_DEBUG("No z coordinate provided for marker %d, assuming 0", id);
z = 0;
}
if (!(s >> yaw)) {
ROS_DEBUG("aruco_map: No yaw provided for marker %d, assuming 0", id);
NODELET_DEBUG("No yaw provided for marker %d, assuming 0", id);
yaw = 0;
}
if (!(s >> pitch)) {
ROS_DEBUG("aruco_map: No pitch provided for marker %d, assuming 0", id);
NODELET_DEBUG("No pitch provided for marker %d, assuming 0", id);
pitch = 0;
}
if (!(s >> roll)) {
ROS_DEBUG("aruco_map: No roll provided for marker %d, assuming 0", id);
NODELET_DEBUG("No roll provided for marker %d, assuming 0", id);
roll = 0;
}
addMarker(id, length, x, y, z, yaw, pitch, roll);
}
ROS_INFO("aruco_map: loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
}
void createGridBoard()
{
ROS_INFO("aruco_map: generate gridboard");
ROS_WARN("aruco_map: gridboard maps are deprecated");
NODELET_INFO("generate gridboard");
NODELET_WARN("gridboard maps are deprecated");
int markers_x, markers_y, first_marker;
double markers_side, markers_sep_x, markers_sep_y;
@@ -349,7 +349,7 @@ publish_debug:
if (nh_priv_.getParam("marker_ids", marker_ids)) {
if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
NODELET_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
ros::shutdown();
}
} else {
@@ -366,7 +366,7 @@ publish_debug:
for(int x = 0; x < markers_x; x++) {
double x_pos = x * (markers_side + markers_sep_x);
double y_pos = max_y - y * (markers_side + markers_sep_y) - markers_side;
ROS_INFO("add marker %d %g %g", marker_ids[y * markers_y + x], x_pos, y_pos);
NODELET_INFO("add marker %d %g %g", marker_ids[y * markers_y + x], x_pos, y_pos);
addMarker(marker_ids[y * markers_y + x], markers_side, x_pos, y_pos, 0, 0, 0, 0);
}
}
@@ -393,14 +393,14 @@ publish_debug:
// Check whether the id is in range for current dictionary
int num_markers = board_->dictionary->bytesList.rows;
if (num_markers <= id) {
ROS_ERROR("aruco_map: Marker id %d is not in dictionary; current dictionary contains %d markers. "
"Please see https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md#parameters for details",
NODELET_ERROR("Marker id %d is not in dictionary; current dictionary contains %d markers. "
"Please see https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md#parameters for details",
id, num_markers);
return;
}
// Check if marker is already in the board
if (std::count(board_->ids.begin(), board_->ids.end(), id) > 0) {
ROS_ERROR("aruco_map: Marker id %d is already in the map", id);
NODELET_ERROR("Marker id %d is already in the map", id);
return;
}
// Create transform

3
clever/launch/clever.launch
View File

@@ -11,6 +11,9 @@
<arg name="led" default="false"/>
<arg name="rc" default="true"/>
<!-- log formatting -->
<env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
<!-- mavros -->
<include file="$(find clever)/launch/mavros.launch">
<arg name="fcu_conn" value="$(arg fcu_conn)"/>

2
clever/src/led.cpp
View File

@@ -315,7 +315,7 @@ int main(int argc, char **argv)
timer = nh.createTimer(ros::Duration(0), &proceed, false, false);
ROS_INFO("led: ready");
ROS_INFO("ready");
notify("startup");
ros::spin();
}

2
clever/src/optical_flow.cpp
View File

@@ -80,7 +80,7 @@ private:
flow_.distance = -1; // no distance sensor available
flow_.temperature = 0;
ROS_INFO("Optical Flow initialized");
NODELET_INFO("Optical Flow initialized");
}
void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo) {

14
clever/src/simple_offboard.cpp
View File

@@ -226,7 +226,7 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
res.pitch = pitch;
res.roll = roll;
} catch (const tf2::TransformException& e) {
ROS_DEBUG("simple_offboard: %s", e.what());
ROS_DEBUG("%s", e.what());
}
if (!TIMEOUT(velocity, velocity_timeout)) {
@@ -273,7 +273,7 @@ void offboardAndArm()
if (state.mode != "OFFBOARD") {
auto start = ros::Time::now();
ROS_INFO("simple_offboard: switch to OFFBOARD");
ROS_INFO("switch to OFFBOARD");
static mavros_msgs::SetMode sm;
sm.request.custom_mode = "OFFBOARD";
@@ -298,7 +298,7 @@ void offboardAndArm()
if (!state.armed) {
ros::Time start = ros::Time::now();
ROS_INFO("simple_offboard: arming");
ROS_INFO("arming");
mavros_msgs::CommandBool srv;
srv.request.value = true;
if (!arming.call(srv)) {
@@ -398,7 +398,7 @@ void publish(const ros::Time stamp)
}
} catch (const tf2::TransformException& e) {
ROS_WARN_THROTTLE(10, "simple_offboard: can't transform");
ROS_WARN_THROTTLE(10, "can't transform");
}
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
@@ -669,7 +669,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
} catch (const std::exception& e) {
message = e.what();
ROS_INFO("simple_offboard: %s", message.c_str());
ROS_INFO("%s", message.c_str());
busy = false;
return true;
}
@@ -745,7 +745,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
} catch (const std::exception& e) {
res.message = e.what();
ROS_INFO("simple_offboard: %s", e.what());
ROS_INFO("%s", e.what());
busy = false;
return true;
}
@@ -821,6 +821,6 @@ int main(int argc, char **argv)
position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
rates_msg.header.frame_id = fcu_frame;
ROS_INFO("simple_offboard: ready");
ROS_INFO("ready");
ros::spin();
}

14
clever/src/vpe_publisher.cpp
View File

@@ -40,7 +40,7 @@ void publishZero(const ros::TimerEvent& e)
if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
if (got_local_pos.isZero()) {
ROS_INFO("vpe_publisher: got local position");
ROS_INFO("got local position");
got_local_pos = e.current_real;
}
@@ -50,7 +50,7 @@ void publishZero(const ros::TimerEvent& e)
got_local_pos = ros::Time(0);
}
ROS_INFO_THROTTLE(10, "vpe_publisher: publish zero");
ROS_INFO_THROTTLE(10, "publish zero");
static geometry_msgs::PoseStamped zero;
zero.header.frame_id = local_frame_id;
zero.header.stamp = e.current_real;
@@ -91,7 +91,7 @@ void callback(const T& msg)
// offset.header.frame_id = vpe.header.frame_id;
offset.child_frame_id = offset_frame_id;
br.sendTransform(offset);
ROS_INFO("vpe_publisher: offset reset");
ROS_INFO("offset reset");
}
// apply the offset
tf2::doTransform(vpe, vpe, offset);
@@ -102,7 +102,7 @@ void callback(const T& msg)
vpe_pub.publish(vpe);
} catch (const tf2::TransformException& e) {
ROS_WARN_THROTTLE(5, "vpe_publisher: %s", e.what());
ROS_WARN_THROTTLE(5, "%s", e.what());
}
}
@@ -119,9 +119,9 @@ int main(int argc, char **argv) {
offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
if (!frame_id.empty()) {
ROS_INFO("vpe_publisher: using data from TF");
ROS_INFO("using data from TF");
} else {
ROS_INFO("vpe_publisher: using data topic");
ROS_INFO("using data topic");
}
auto pose_sub = nh_priv.subscribe<PoseStamped>("pose", 1, &callback);
@@ -139,6 +139,6 @@ int main(int argc, char **argv) {
local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
}
ROS_INFO("vpe_publisher: ready");
ROS_INFO("ready");
ros::spin();
}