Skip to content
This repository was archived by the owner on Jul 9, 2024. It is now read-only.
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
1 change: 1 addition & 0 deletions moveit_workspace_analysis/launch/reader.launch
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
<launch>
<node name="moveit_workspace_reader" pkg="moveit_workspace_analysis" type="workspace_reader_node" respawn="false" output="screen">
<param name="marker_scale" value="0.20" />
<param name="filename" value="$(env ROS_LOG_DIR)/workspace_analysis_results.txt" />
</node>
</launch>
5 changes: 4 additions & 1 deletion moveit_workspace_analysis/src/reader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,11 @@ int main(int argc, char **argv)
/*Get some ROS params */
ros::NodeHandle node_handle("~");
std::string filename;
double marker_scale;
if (!node_handle.getParam("filename", filename))
ROS_FATAL("No filename to read from");
if (!node_handle.getParam("marker_scale", marker_scale))
marker_scale = 0.20;

moveit_workspace_analysis::WorkspaceMetrics metrics;

Expand All @@ -66,7 +69,7 @@ int main(int argc, char **argv)
ROS_INFO("Could not write to file");
*/
ros::Publisher display_publisher = node_handle.advertise<visualization_msgs::Marker>("workspace", 1, true);
visualization_msgs::Marker marker = metrics.getMarker(0.25, 0, "me");
visualization_msgs::Marker marker = metrics.getMarker(marker_scale, 0, "me");
marker.header.frame_id = metrics.frame_id_;
marker.header.stamp = ros::Time::now();
display_publisher.publish(marker);
Expand Down