Package Summary

Tags No category tags.
Version 3.1.0
License BSD
Build type CATKIN
Use RECOMMENDED

Repository Summary

Checkout URI https://github.com/davetcoleman/moveit_visual_tools.git
VCS Type git
VCS Version jade-devel
Last Updated 2016-06-14
Dev Status DEVELOPED
CI status Continuous Integration
Released RELEASED
Tags No category tags.
Contributing Help Wanted (0)
Good First Issues (0)
Pull Requests to Review (0)

Package Description

Helper functions for displaying and debugging MoveIt! data in Rviz via published markers

Additional Links

Maintainers

  • Dave Coleman

Authors

  • Dave Coleman

MoveIt! Visual Tools

Helper functions for displaying and debugging MoveIt! data in Rviz via published markers, trajectories, and MoveIt! collision objects. It is sometimes hard to understand everything that is going on internally with MoveIt!, but using these quick convenience functions allows one to easily visualize their code.

This package helps you visualize:

  • Basic Rviz geometric shapes
  • MoveIt! collision objects
  • MoveIt! and ROS trajectories
  • Robot states
  • End effectors

Developed by Dave Coleman at the Correll Robotics Lab, University of Colorado Boulder with outside contributors.

  • Build Status Travis CI
  • Build Status ROS Buildfarm - Trusty Devel Source Build
  • Build Status ROS Buildfarm - AMD64 Trusty Debian Build

Install

Ubuntu Debian

sudo apt-get install ros-indigo-moveit-visual-tools

Install From Source

Clone this repository into a catkin workspace, then use the rosdep install tool to automatically download its dependencies. Depending on your current version of ROS, use:

rosdep install --from-paths src --ignore-src --rosdistro indigo

Quick Start Demo

First launch Rviz:

roslaunch moveit_visual_tools demo_rviz.launch

Then run some demos displaying robot states and collision objects:

roslaunch moveit_visual_tools demo.launch

This package is built in top of rviz_visual_tools and all those features are included. Try running the rviz demo documented on this README.

Code API

See VisualTools Class Reference

Usage

We'll assume you will be using these helper functions within a class.

Initialize

Add to your includes:

#include <moveit_visual_tools/visual_tools.h>

Add to your class's member variables:

// For visualizing things in rviz
moveit_visual_tools::VisualToolsPtr visual_tools_;

In your class' constructor add:

visual_tools_.reset(new moveit_visual_tools::VisualTools("base_frame","/moveit_visual_markers"));

Collision Object Functions

Helpers for adding and removing objects from the MoveIt! planning scene. CO stands for Collision Object and ACO stands for Active Collision Object.

  • cleanupCO
  • cleanupACO
  • attachCO
  • publishCollisionBlock
  • publishCollisionCylinder
  • publishCollisionTree
  • publishCollisionTable
  • publishCollisionWall

And more...

Animate Trajectories

Higher level robot and trajectory functions

  • publishTrajectoryPath
  • publishTrajectoryPoint
  • publishRobotState
  • publishAnimatedGrasps
  • publishIKSolutions

Show parts of a robot

These functions are a little more complicated TODO document more

  • publishEEMarkers

Parent Class

This class is built on top of rviz_visual_tools so all features and documentation for that package apply here as well.

Developers Notes

Useful notes for anyone wanting to dig in deeper:

  • All poses are published with respect to the world frame e.g. /world, /odom, or maybe /base
  • All publish() ROS topics should be followed by a ros::spinOnce(); but no sleep
  • Do not want to load any features/publishers until they are actually needed since this library contains so many components

Testing and Linting

To run roslint, use the following command with catkin-tools:

catkin build --no-status --no-deps --this --make-args roslint

To run catkin lint, use the following command with catkin-tools:

catkin lint -W2

Use the following command with catkin-tools to run the small amount of available tests:

catkin run_tests --no-deps --this -i

Contribute

Please send PRs for new helper functions, fixes, etc!

CHANGELOG

Changelog for package moveit_visual_tools

3.1.0 (2016-04-28)

  • Re-factored and fixed visual tools demo!
  • Fixes for catkin lint
  • Fixes for roslint
  • Removed deprecated function call
  • Remove deprecated test
  • New root_robot_state utilization
  • Ablity to move a RobotState\'s root frame permenatly around in the scene
  • Better publishCollisionWall() function
  • Deprecated old publishTrajectoryLine() functions - removed clear_all_markers argument
  • New publishTrajectoryPath() variant
  • Rename namespace of RobotState
  • Made INFO into DEBUG output
  • New publishTrajectoryLine function
  • Switched publishTrajectoryLine to use cylinders instead of lines
  • New showJointLimits() function for console debugging a robot state
  • Fix publishTrajectoryPath() bug
  • Default blocking time for trajectory if not parameterized
  • Publish workspace parameters was incorrectly creating a collision object
  • Contributors: Dave Coleman

3.0.5 (2016-02-09)

  • Updated README
  • Better comment
  • Contributors: Dave Coleman

3.0.4 (2016-01-12)

  • Removed stray debug output
  • Improved debugging output for the hideRobot() feature and virtual_joints
  • Contributors: Dave Coleman

3.0.3 (2016-01-10)

  • Renamed test to demo
  • New publishTrajectoryLine() function
  • Fix travis
  • Deprecated loadEEMarker() that uses string
  • Formatted code
  • Switched from MOVEIT deprecated to RVIZ_VISUAL_TOOLS deprecated
  • Fixed shared_robot_state to initialize correctly every time
  • Switched to using name_ variables
  • Add error checks to publishTrajectoryLine
  • Added ability for publishTrajectoryLine to clear all previous markers
  • Contributors: Dave Coleman

3.0.2 (2015-12-27)

  • Updated README
  • Temp fix missing variable
  • Contributors: Dave Coleman

3.0.1 (2015-12-05)

  • catkin lint cleanup
  • Fix travis
  • Contributors: Dave Coleman

3.0.0 (2015-12-02)

  • Release 3.0
  • Added travis support
  • fix the how to link a demo img
  • Updated link to Doxygen API description
  • Formatting and better debug output
  • Fix hide robot bug
  • Remove incompatible humanoid function
  • Default color when publishing collision meshes
  • Added error check for bad value
  • API change for removal of shape_tools
  • New publish trajectory line function
  • Remove slash from topic name
  • Removed mute functionality
  • Improved loading efficiency
  • publishContactPoints accepts a color
  • Change topics to default when opening Rviz
  • New publishCollisionMesh() function
  • Changed publishCollisionMesh() API
  • Renamed publishCollisionRectangle to publishCollisionCuboid()
  • Updated rviz_visual_tools API
  • New publishMesh from ROS msg function
  • publishRobotState() for a RobotStateMsg now allows color
  • publishTrajectoryPath() for a ROS msg now requires a RobotState
  • New method for attaching collision objects that does not require a publisher
  • Specify scene name and cleanup logging
  • Fixed error checking for hideRobot() function
  • loadTrajectoryPub() allows custom topic
  • New publishTrajectoryPoints() function
  • New publishContactPoints function
  • New publishTrajectoryPath() function
  • New getRobotModel() function
  • New ability to visualize IK solutions with arbitrary virtual joint
  • API Broken: ability to have different end effectors for different arms, auto EE marker loading
  • Publish collision meshes
  • Added check for virtual joint
  • Fixed which arrow gets published
  • Publish fixed link arrows to show footstep locations
  • Ability to specify robot_state_topic without loading the publisher
  • Contributors: Dave Coleman, Daiki Maekawa, simonschmeisser

2.2.0 (2015-01-07)

  • Code cleanup
  • Improved naming
  • Joint model bug fix
  • Improved speed of sending collision objects to Rviz Added Manual planning scene update mode Ability to apply colors to all collision objects (YAY) API: removed removeAllCollisionObjectsPS function Removed loadPlanningPub() function Removed publishRemoveAllCollisionObjects() function
  • Added backwards compatibile loadCollisionSceneFromFile()
  • New publishCollisionRectangle function API: Changed loadCollisionSceneFromFile() to accept a pose instead of x,y
  • Fix for renamed function
  • New publishWorkspaceParameters() function
  • Added ability to publish robot states with color
  • Fixed install method
  • Merge pull request #5 from robomakery/feature/fix-collision-objects-test
  • Fixes for missing declarations in collision_objects_test.cpp
  • Refactored how collision ojects are published Created new collision objects test and roslaunch file Optimized header file Removed loadCollisionPub() function Fixed publishCollisionFloor Added publishCollisionRectangle
  • Contributors: Dave Coleman, Dylan Vaughn

2.1.0 (2014-10-31)

  • Fix for upstream change of RvizVisualTools
  • Set animation speed of grasps
  • Fix publishing end effector
  • New publishCollisionObjectMsg() function
  • New getSharedRobotState() accessor function
  • Consolidated publish marker functions
  • Fixed loadEEMarker() to be called more than once
  • Contributors: Dave Coleman

2.0.0 (2014-10-27)

  • Updated README
  • API Upgrade Notes
  • Renamed to have \'MoveIt\' prefix in class and file name, moved base functionality to rviz_visual_tools
  • Added new publishSphere function and publish_sphere test script
  • Created better test script
  • Better static_id handling for publishText
  • Added mainpage for API docs
  • Enabled colors
  • Improved integer random num generation
  • New publishSpheres functions
  • Contributors: Dave Coleman

1.3.0 (2014-09-17)

  • Added new getRandColor() function
  • Added TRANSLUCENT color
  • Added two new publishSphere() functions
  • New convertPointToPose function
  • Reduced sleep timer for starting all publishers from 0.5 seconds to 0.2 seconds
  • Removed stacktrace tool because already exists in moveit_core
  • New publishText function that allows custom scale and id number be passed in
  • Removed deprecated getEEParentLink() function
  • Added new scale sizes
  • Added new processCollisionObvMsg()
  • Added new setPlanningSceneMonitor()
  • Deprecated removeAllColisionObejcts()
  • Created new removeAllCollisionObjectsPS()
  • Added new publishCollisionFloor()
  • Added new loadCollisionSceneFromFile()
  • New color purple
  • Added new setBaseFrame() function
  • Contributors: Dave Coleman

1.2.1 (2014-08-11)

  • Renamed base_link to base_frame
  • Added new getBaseFrame() function
  • Deprecated getBaseLink() function
  • Contributors: Dave Coleman

1.2.0 (2014-08-08)

  • Added XXLarge size
  • Added global_scale feature
  • Added hideRobot() functionality
  • Added removeAllCollisionObjects from planning scene monitor
  • Added publishCollisionSceneFromFile function
  • Formatting
  • Contributors: Dave Coleman

1.1.0 (2014-07-31)

  • Bug fixes
  • Fixed convertPoint32ToPose
  • Added scale to publishText
  • New publishPolygon, publishMarker, convertPose, convertPointToPose, and convertPoint32 functions
  • New deleteAllMarkers, publishPath, publishSpheres, and convertPoseToPoint functions
  • Added getCollisionWall
  • Made lines darker
  • Added reset marker feature
  • Namespaces for publishSphere
  • New publishTrajectory function
  • Merging features from OMPL viewer
  • Refactored functions, new robot_model intialization
  • Added more rand functions and made them static
  • Added graph_msgs generated messages dependence so it waits for it to be compiled
  • Updated README
  • Contributors: Dave Coleman, Sammy Pfeiffer

1.0.1 (2014-05-30)

  • Updated README
  • Indigo support
  • Fix for strict cppcheck and g++ warnings/errors
  • Compatibilty fix for Eigen package in ROS Indigo
  • Fix uninitialized
  • Fix functions with no return statement and other cppcheck errors
  • Contributors: Bence Magyar, Dave Coleman, Jordi Pages

1.0.0 (2014-05-05)

  • Enabled dual arm manipulation
  • Removed notions of a global planning group, ee group name, or ee parent link.
  • Changed functionality of loadEEMarker
  • Added new print function
  • Made getPlanningSceneMonitor() private function
  • Renamed loadPathPub()
  • Added tool for visualizing unmangled stack trace
  • Created function for publishing non-animated grasps
  • Created new publishGraph function. Renamed publishCollisionTree to publishCollisionGraph
  • Created functions for loading publishers with a delay
  • Removed old method of removing all collision objects
  • Created better testing functionality
  • Changed return type from void to bool for many functions
  • Changed way trajectory is timed
  • Created new publishIKSolutions() function for grasp poses, etc
  • Added new MoveIt robot state functionality
  • Added visualize grasp functionality
  • Removed unnecessary run dependencies
  • Updated README

0.2.0 (2014-04-11)

  • Improved header comments are re-ordered functions into groups
  • Started to create new trajectory point publisher
  • Added getBaseLink function
  • Added dependency on graph_msgs
  • Added new collision cylinder functionality
  • Created example code in README
  • Renamed visualization to visual keyword
  • Updated README

0.1.0 (2014-04-04)

  • Split moveit_visual_tools from its original usage within block_grasp_generator package

Wiki Tutorials

See ROS Wiki Tutorials for more details.

Source Tutorials

Not currently indexed.

Launch files

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged moveit_visual_tools at Robotics Stack Exchange

Package Summary

Tags No category tags.
Version 3.0.5
License BSD
Build type CATKIN
Use RECOMMENDED

Repository Summary

Checkout URI https://github.com/davetcoleman/moveit_visual_tools.git
VCS Type git
VCS Version indigo-devel
Last Updated 2016-02-09
Dev Status DEVELOPED
CI status Continuous Integration
Released RELEASED
Tags No category tags.
Contributing Help Wanted (0)
Good First Issues (0)
Pull Requests to Review (0)

Package Description

Helper functions for displaying and debugging MoveIt! data in Rviz via published markers

Additional Links

Maintainers

  • Dave Coleman

Authors

  • Dave Coleman

MoveIt! Visual Tools

Helper functions for displaying and debugging MoveIt! data in Rviz via published markers, trajectories, and MoveIt! collision objects. It is sometimes hard to understand everything that is going on internally with MoveIt!, but using these quick convenience functions allows one to easily visualize their code.

This package includes:

  • Basic geometric markers for Rviz
  • MoveIt! collision object tools
  • Trajectory visualization tools
  • Robot state tools

Developed by Dave Coleman at the Correll Robotics Lab, University of Colorado Boulder with outside contributors.

  • Build Status Travis CI
  • Build Status ROS Buildfarm - Trusty Devel Source Build
  • Build Status ROS Buildfarm - AMD64 Trusty Debian Build

Install

Ubuntu Debian

sudo apt-get install ros-indigo-moveit-visual-tools

Install From Source

Clone this repository into a catkin workspace, then use the rosdep install tool to automatically download its dependencies. Depending on your current version of ROS, use:

rosdep install --from-paths src --ignore-src --rosdistro indigo

Quick Start Demo

To see random shapes generated in Rviz:

roslaunch moveit_visual_tools visual_tools_demo.launch

You should see something like:

You can also demo the collision objects generation TODO: THIS IS CURRENTLY BROKEN :(

roslaunch moveit_visual_tools collision_objects_demo_rviz.launch
roslaunch moveit_visual_tools collision_objects_demo.launch

Code API

See VisualTools Class Reference

Upgrade Notes From Indigo

We recently did a major refactor of moveit_visual_tools in Indigo that caused some API breaking changes. If you do not want to bother, you can still build the old version from source using the branch indigo-devel-old-api.

To upgrade, do the following (or use the upgrade script further down):

Orignal API New API
#include <moveit_visual_tools/visual_tools.h> #include <moveit_visual_tools/moveit_visual_tools.h>
moveit_visual_tools::VisualTools moveit_visual_tools::MoveItVisualTools
moveit_visual_tools::VisualToolsPtr moveit_visual_tools::MoveItVisualToolsPtr
moveit_visual_tools::rviz_colors rviz_visual_tools::colors
moveit_visual_tools::rviz_scales rviz_visual_tools::scales

Auto Upgrade Script

Run each line in order in the /src of your catkin workspace:

findreplace() { grep -lr -e "$1" * | xargs sed -i "s/$1/$2/g" ;}
findreplace '<moveit_visual_tools\/visual_tools.h>' '<moveit_visual_tools\/moveit_visual_tools.h>'
findreplace moveit_visual_tools::VisualTools moveit_visual_tools::MoveItVisualTools
findreplace 'moveit_visual_tools::' 'rviz_visual_tools::'
findreplace 'rviz_visual_tools::MoveItVisualTools' 'moveit_visual_tools::MoveItVisualTools'

Usage

We'll assume you will be using these helper functions within a class.

Initialize

Add to your includes:

#include <moveit_visual_tools/visual_tools.h>

Add to your class's member variables:

// For visualizing things in rviz
moveit_visual_tools::VisualToolsPtr visual_tools_;

In your class' constructor add:

visual_tools_.reset(new moveit_visual_tools::VisualTools("base_frame","/moveit_visual_markers"));

Change the first parameter to the name of your robot's base frame, and the second parameter to whatever name you'd like to use for the corresponding Rviz marker ROS topic.

There are several other settings you can adjust, which I might get around to documenting in the future:

visual_tools_->setMuted(false);
visual_tools_->setLifetime(20.0);
visual_tools_->setEEGroupName(grasp_data_.ee_group_);
visual_tools_->setPlanningGroupName(planning_group_name_);
visual_tools_->setFloorToBaseHeight(floor_to_base_height);
visual_tools_->setGraspPoseToEEFPose(grasp_pose_to_eef_pose);
visual_tools_->setAlpha(alpha);
visual_tools_->setGlobalScale(scale);
visual_tools_->setBaseFrame(frame_name);

Tools

Now in your code you can easily debug your MoveIt! code using visual markers in Rviz

Start rviz and create a new marker using the 'Add' button at the bottom right. Choose the marker topic to be the same as the topic you specified in the constructor.

Example Code

In the following snippet we create a pose at xyz (0.1, 0.1, 0.1) and rotate the pose down 45 degrees along the Y axis. Then we publish the pose as a arrow for visualziation in Rviz. Make sure your Rviz fixed frame is the same as the one chosen in the code.

// Create pose
Eigen::Affine3d pose;
pose = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY()); // rotate along X axis by 45 degrees
pose.translation() = Eigen::Vector3d( 0.1, 0.1, 0.1 ); // translate x,y,z

// Publish arrow vector of pose
ROS_INFO_STREAM_NAMED("test","Publishing Arrow");
visual_tools_->publishArrow(pose, moveit_visual_tools::RED, moveit_visual_tools::LARGE);

Basic Publishing Functions

See visual_tools.h for more details and documentation on the following functions:

  • publishSphere
  • publishArrow
  • publishCuboid
  • publishLine
  • publishBlock
  • publishText
  • publishTest

And more...

Collision Object Functions

Helpers for adding and removing objects from the MoveIt! planning scene. CO stands for Collision Object and ACO stands for Active Collision Object.

  • cleanupCO
  • cleanupACO
  • attachCO
  • publishCollisionBlock
  • publishCollisionCylinder
  • publishCollisionTree
  • publishCollisionTable
  • publishCollisionWall

And more...

Animate Trajectories

Higher level robot and trajectory functions

  • publishTrajectoryPath
  • publishTrajectoryPoint
  • publishRobotState
  • publishAnimatedGrasps
  • publishIKSolutions

Show parts of a robot

These functions are a little more complicated TODO document more

  • publishEEMarkers

Helper Functions

Reset function

  • deleteAllMarkers - tells Rviz to clear out all current markers from being displayed. Only withs in ROS Indigo and newer.

Conversion functions

  • convertPose
  • convertPoint32ToPose
  • convertPoseToPoint
  • convertPoint
  • convertPoint32

Convenience functions

  • generateRandomPose
  • dRand
  • fRand
  • iRand
  • getCenterPoint
  • getVectorBetweenPoints

Available Colors

This package helps you quickly choose colors - feel free to send PRs with more colors as needed

  • moveit_visual_tools::RED
  • moveit_visual_tools::GREEN
  • moveit_visual_tools::BLUE
  • moveit_visual_tools::GREY
  • moveit_visual_tools::WHITE
  • moveit_visual_tools::ORANGE
  • moveit_visual_tools::BLACK
  • moveit_visual_tools::YELLOW

Available Marker Sizes

  • moveit_visual_tools::XXSMALL
  • moveit_visual_tools::XSMALL
  • moveit_visual_tools::SMALL
  • moveit_visual_tools::REGULAR
  • moveit_visual_tools::LARGE
  • moveit_visual_tools::XLARGE

Lifetime

All markers will persist for the duration set by setLifetime, defaulting to 30 seconds. You can reset this earlier by calling

resetMarkerCounts();

This will cause all new markers to overwrite older ones.

You can also delete all markers (new in ROS Indigo) by calling

deleteAllMarkers();

Developers Notes

Useful notes for anyone wanting to dig in deeper:

  • All poses are published with respect to the world frame e.g. /world, /odom, or maybe /base
  • All publish() ROS topics should be followed by a ros::spinOnce(); but no sleep
  • Do not want to load any features/publishers until they are actually needed since this library contains so many components

Contribute

Feel free to send PRs for new helper functions, fixes, etc. - I'll happily discuss and merge them. I do not, however, want to send much time helping people use this because I am a busy grad student. Use at your own risk.

CHANGELOG

Changelog for package moveit_visual_tools

3.0.5 (2016-02-09)

  • Updated README
  • Better comment
  • Contributors: Dave Coleman

3.0.4 (2016-01-12)

  • Removed stray debug output
  • Improved debugging output for the hideRobot() feature and virtual_joints
  • Contributors: Dave Coleman

3.0.3 (2016-01-10)

  • Renamed test to demo
  • New publishTrajectoryLine() function
  • Fix travis
  • Deprecated loadEEMarker() that uses string
  • Formatted code
  • Switched from MOVEIT deprecated to RVIZ_VISUAL_TOOLS deprecated
  • Fixed shared_robot_state to initialize correctly every time
  • Switched to using name_ variables
  • Add error checks to publishTrajectoryLine
  • Added ability for publishTrajectoryLine to clear all previous markers
  • Contributors: Dave Coleman

3.0.2 (2015-12-27)

  • Updated README
  • Temp fix missing variable
  • Contributors: Dave Coleman

3.0.1 (2015-12-05)

  • catkin lint cleanup
  • Fix travis
  • Contributors: Dave Coleman

3.0.0 (2015-12-02)

  • Release 3.0
  • Added travis support
  • fix the how to link a demo img
  • Updated link to Doxygen API description
  • Formatting and better debug output
  • Fix hide robot bug
  • Remove incompatible humanoid function
  • Default color when publishing collision meshes
  • Added error check for bad value
  • API change for removal of shape_tools
  • New publish trajectory line function
  • Remove slash from topic name
  • Removed mute functionality
  • Improved loading efficiency
  • publishContactPoints accepts a color
  • Change topics to default when opening Rviz
  • New publishCollisionMesh() function
  • Changed publishCollisionMesh() API
  • Renamed publishCollisionRectangle to publishCollisionCuboid()
  • Updated rviz_visual_tools API
  • New publishMesh from ROS msg function
  • publishRobotState() for a RobotStateMsg now allows color
  • publishTrajectoryPath() for a ROS msg now requires a RobotState
  • New method for attaching collision objects that does not require a publisher
  • Specify scene name and cleanup logging
  • Fixed error checking for hideRobot() function
  • loadTrajectoryPub() allows custom topic
  • New publishTrajectoryPoints() function
  • New publishContactPoints function
  • New publishTrajectoryPath() function
  • New getRobotModel() function
  • New ability to visualize IK solutions with arbitrary virtual joint
  • API Broken: ability to have different end effectors for different arms, auto EE marker loading
  • Publish collision meshes
  • Added check for virtual joint
  • Fixed which arrow gets published
  • Publish fixed link arrows to show footstep locations
  • Ability to specify robot_state_topic without loading the publisher
  • Contributors: Dave Coleman, Daiki Maekawa, simonschmeisser

2.2.0 (2015-01-07)

  • Code cleanup
  • Improved naming
  • Joint model bug fix
  • Improved speed of sending collision objects to Rviz Added Manual planning scene update mode Ability to apply colors to all collision objects (YAY) API: removed removeAllCollisionObjectsPS function Removed loadPlanningPub() function Removed publishRemoveAllCollisionObjects() function
  • Added backwards compatibile loadCollisionSceneFromFile()
  • New publishCollisionRectangle function API: Changed loadCollisionSceneFromFile() to accept a pose instead of x,y
  • Fix for renamed function
  • New publishWorkspaceParameters() function
  • Added ability to publish robot states with color
  • Fixed install method
  • Merge pull request #5 from robomakery/feature/fix-collision-objects-test
  • Fixes for missing declarations in collision_objects_test.cpp
  • Refactored how collision ojects are published Created new collision objects test and roslaunch file Optimized header file Removed loadCollisionPub() function Fixed publishCollisionFloor Added publishCollisionRectangle
  • Contributors: Dave Coleman, Dylan Vaughn

2.1.0 (2014-10-31)

  • Fix for upstream change of RvizVisualTools
  • Set animation speed of grasps
  • Fix publishing end effector
  • New publishCollisionObjectMsg() function
  • New getSharedRobotState() accessor function
  • Consolidated publish marker functions
  • Fixed loadEEMarker() to be called more than once
  • Contributors: Dave Coleman

2.0.0 (2014-10-27)

  • Updated README
  • API Upgrade Notes
  • Renamed to have \'MoveIt\' prefix in class and file name, moved base functionality to rviz_visual_tools
  • Added new publishSphere function and publish_sphere test script
  • Created better test script
  • Better static_id handling for publishText
  • Added mainpage for API docs
  • Enabled colors
  • Improved integer random num generation
  • New publishSpheres functions
  • Contributors: Dave Coleman

1.3.0 (2014-09-17)

  • Added new getRandColor() function
  • Added TRANSLUCENT color
  • Added two new publishSphere() functions
  • New convertPointToPose function
  • Reduced sleep timer for starting all publishers from 0.5 seconds to 0.2 seconds
  • Removed stacktrace tool because already exists in moveit_core
  • New publishText function that allows custom scale and id number be passed in
  • Removed deprecated getEEParentLink() function
  • Added new scale sizes
  • Added new processCollisionObvMsg()
  • Added new setPlanningSceneMonitor()
  • Deprecated removeAllColisionObejcts()
  • Created new removeAllCollisionObjectsPS()
  • Added new publishCollisionFloor()
  • Added new loadCollisionSceneFromFile()
  • New color purple
  • Added new setBaseFrame() function
  • Contributors: Dave Coleman

1.2.1 (2014-08-11)

  • Renamed base_link to base_frame
  • Added new getBaseFrame() function
  • Deprecated getBaseLink() function
  • Contributors: Dave Coleman

1.2.0 (2014-08-08)

  • Added XXLarge size
  • Added global_scale feature
  • Added hideRobot() functionality
  • Added removeAllCollisionObjects from planning scene monitor
  • Added publishCollisionSceneFromFile function
  • Formatting
  • Contributors: Dave Coleman

1.1.0 (2014-07-31)

  • Bug fixes
  • Fixed convertPoint32ToPose
  • Added scale to publishText
  • New publishPolygon, publishMarker, convertPose, convertPointToPose, and convertPoint32 functions
  • New deleteAllMarkers, publishPath, publishSpheres, and convertPoseToPoint functions
  • Added getCollisionWall
  • Made lines darker
  • Added reset marker feature
  • Namespaces for publishSphere
  • New publishTrajectory function
  • Merging features from OMPL viewer
  • Refactored functions, new robot_model intialization
  • Added more rand functions and made them static
  • Added graph_msgs generated messages dependence so it waits for it to be compiled
  • Updated README
  • Contributors: Dave Coleman, Sammy Pfeiffer

1.0.1 (2014-05-30)

  • Updated README
  • Indigo support
  • Fix for strict cppcheck and g++ warnings/errors
  • Compatibilty fix for Eigen package in ROS Indigo
  • Fix uninitialized
  • Fix functions with no return statement and other cppcheck errors
  • Contributors: Bence Magyar, Dave Coleman, Jordi Pages

1.0.0 (2014-05-05)

  • Enabled dual arm manipulation
  • Removed notions of a global planning group, ee group name, or ee parent link.
  • Changed functionality of loadEEMarker
  • Added new print function
  • Made getPlanningSceneMonitor() private function
  • Renamed loadPathPub()
  • Added tool for visualizing unmangled stack trace
  • Created function for publishing non-animated grasps
  • Created new publishGraph function. Renamed publishCollisionTree to publishCollisionGraph
  • Created functions for loading publishers with a delay
  • Removed old method of removing all collision objects
  • Created better testing functionality
  • Changed return type from void to bool for many functions
  • Changed way trajectory is timed
  • Created new publishIKSolutions() function for grasp poses, etc
  • Added new MoveIt robot state functionality
  • Added visualize grasp functionality
  • Removed unnecessary run dependencies
  • Updated README

0.2.0 (2014-04-11)

  • Improved header comments are re-ordered functions into groups
  • Started to create new trajectory point publisher
  • Added getBaseLink function
  • Added dependency on graph_msgs
  • Added new collision cylinder functionality
  • Created example code in README
  • Renamed visualization to visual keyword
  • Updated README

0.1.0 (2014-04-04)

  • Split moveit_visual_tools from its original usage within block_grasp_generator package

Wiki Tutorials

See ROS Wiki Tutorials for more details.

Source Tutorials

Not currently indexed.

Launch files

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged moveit_visual_tools at Robotics Stack Exchange

Package Summary

Tags No category tags.
Version 1.3.0
License BSD
Build type CATKIN
Use RECOMMENDED

Repository Summary

Checkout URI https://github.com/davetcoleman/moveit_visual_tools.git
VCS Type git
VCS Version hydro-devel
Last Updated 2014-09-17
Dev Status DEVELOPED
CI status Continuous Integration
Released RELEASED
Tags No category tags.
Contributing Help Wanted (0)
Good First Issues (0)
Pull Requests to Review (0)

Package Description

Helper functions for displaying and debugging MoveIt! data in Rviz via published markers

Additional Links

Maintainers

  • Dave Coleman

Authors

  • Dave Coleman

MoveIt! Visual Tools

Helper functions for displaying and debugging MoveIt! data in Rviz via published markers, trajectories, and MoveIt! collision objects. It is sometimes hard to understand everything that is going on internally with MoveIt!, but using these quick convenience functions allows one to easily visualize their code.

This package includes:

  • Basic geometric markers for Rviz
  • MoveIt! collision object tools
  • Trajectory visualization tools
  • Robot state tools

Developed by Dave Coleman at the Correll Robotics Lab, University of Colorado Boulder with outside contributors.

Build Status

Build Status

Install

Ubuntu Debian

sudo apt-get install ros-hydro-moveit-visual-tools
sudo apt-get install ros-indigo-moveit-visual-tools

Install From Source

Clone this repository into a catkin workspace, then use the rosdep install tool to automatically download its dependencies. Depending on your current version of ROS, use:

rosdep install --from-paths src --ignore-src --rosdistro indigo

Usage

We'll assume you will be using these helper functions within a class.

Initialize

Add to your includes:

#include <moveit_visual_tools/visual_tools.h>

Add to your class's member variables:

// For visualizing things in rviz
moveit_visual_tools::VisualToolsPtr visual_tools_;

In your class' constructor add:

visual_tools_.reset(new moveit_visual_tools::VisualTools("base_frame","/moveit_visual_markers"));

Change the first parameter to the name of your robot's base frame, and the second parameter to whatever name you'd like to use for the corresponding Rviz marker ROS topic.

There are several other settings you can adjust, which I might get around to documenting in the future:

visual_tools_->setMuted(false);
visual_tools_->setLifetime(20.0);
visual_tools_->setEEGroupName(grasp_data_.ee_group_);
visual_tools_->setPlanningGroupName(planning_group_name_);
visual_tools_->setFloorToBaseHeight(floor_to_base_height);
visual_tools_->setGraspPoseToEEFPose(grasp_pose_to_eef_pose);
visual_tools_->setAlpha(alpha);
visual_tools_->setGlobalScale(scale);
visual_tools_->setBaseFrame(frame_name);

Tools

Now in your code you can easily debug your MoveIt! code using visual markers in Rviz

Start rviz and create a new marker using the 'Add' button at the bottom right. Choose the marker topic to be the same as the topic you specified in the constructor.

Example Code

In the following snippet we create a pose at xyz (0.1, 0.1, 0.1) and rotate the pose down 45 degrees along the Y axis. Then we publish the pose as a arrow for visualziation in Rviz. Make sure your Rviz fixed frame is the same as the one chosen in the code.

// Create pose
Eigen::Affine3d pose;
pose = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY()); // rotate along X axis by 45 degrees
pose.translation() = Eigen::Vector3d( 0.1, 0.1, 0.1 ); // translate x,y,z

// Publish arrow vector of pose
ROS_INFO_STREAM_NAMED("test","Publishing Arrow");
visual_tools_->publishArrow(pose, moveit_visual_tools::RED, moveit_visual_tools::LARGE);

Basic Publishing Functions

See visual_tools.h for more details and documentation on the following functions:

  • publishSphere
  • publishArrow
  • publishRectangle
  • publishLine
  • publishBlock
  • publishText
  • publishTest

And more...

Collision Object Functions

To

Helpers for adding and removing objects from the MoveIt! planning scene. CO stands for Collision Object and ACO stands for Active Collision Object.

DEVELOPER TODO: make it so that to use these functions, you must first instanciate a planning scene monitor outside of moveit_visual_tools. Remove publish collision message

  • cleanupCO
  • cleanupACO
  • attachCO
  • publishCollisionBlock
  • publishCollisionCylinder
  • publishCollisionTree
  • publishCollisionTable
  • publishCollisionWall

And more...

Animate Trajectories

Higher level robot and trajectory functions

  • publishTrajectoryPath
  • publishTrajectoryPoint
  • publishRobotState
  • publishAnimatedGrasps
  • publishIKSolutions

Show parts of a robot

These functions are a little more complicated TODO document more

  • publishEEMarkers

Helper Functions

Reset function

  • deleteAllMarkers - tells Rviz to clear out all current markers from being displayed. Only withs in ROS Indigo and newer.

Conversion functions

  • convertPose
  • convertPoint32ToPose
  • convertPoseToPoint
  • convertPoint
  • convertPoint32

Convenience functions

  • generateRandomPose
  • dRand
  • fRand
  • iRand
  • getCenterPoint
  • getVectorBetweenPoints

Available Colors

This package helps you quickly choose colors - feel free to send PRs with more colors as needed

  • moveit_visual_tools::RED
  • moveit_visual_tools::GREEN
  • moveit_visual_tools::BLUE
  • moveit_visual_tools::GREY
  • moveit_visual_tools::WHITE
  • moveit_visual_tools::ORANGE
  • moveit_visual_tools::BLACK
  • moveit_visual_tools::YELLOW

Available Marker Sizes

  • moveit_visual_tools::XXSMALL
  • moveit_visual_tools::XSMALL
  • moveit_visual_tools::SMALL
  • moveit_visual_tools::REGULAR
  • moveit_visual_tools::LARGE
  • moveit_visual_tools::XLARGE

Lifetime

All markers will persist for the duration set by setLifetime, defaulting to 30 seconds. You can reset this earlier by calling

resetMarkerCounts();

This will cause all new markers to overwrite older ones.

You can also delete all markers (new in ROS Indigo) by calling

deleteAllMarkers();

Developers Notes

Useful notes for anyone wanting to dig in deeper:

  • All poses are published with respect to the world frame e.g. /world, /odom, or maybe /base
  • All publish() ROS topics should be followed by a ros::spinOnce(); but no sleep
  • Do not want to load any features/publishers until they are actually needed since this library contains so many components

Contribute

Feel free to send PRs for new helper functions, fixes, etc. - I'll happily discuss and merge them. I do not, however, want to send much time helping people use this because I am a busy grad student. Use at your own risk.

CHANGELOG

Changelog for package moveit_visual_tools

1.3.0 (2014-09-17)

  • Added new getRandColor() function
  • Added TRANSLUCENT2 color
  • Added two new publishSphere() functions
  • New convertPointToPose function
  • Reduced sleep timer for starting all publishers from 0.5 seconds to 0.2 seconds
  • Removed stacktrace tool because already exists in moveit_core
  • New publishText function that allows custom scale and id number be passed in
  • Removed deprecated getEEParentLink() function
  • Added new scale sizes
  • Added new processCollisionObvMsg()
  • Added new setPlanningSceneMonitor()
  • Deprecated removeAllColisionObejcts()
  • Created new removeAllCollisionObjectsPS()
  • Added new publishCollisionFloor()
  • Added new loadCollisionSceneFromFile()
  • New color purple
  • Added new setBaseFrame() function
  • Contributors: Dave Coleman

1.2.1 (2014-08-11)

  • Renamed base_link to base_frame
  • Added new getBaseFrame() function
  • Deprecated getBaseLink() function
  • Contributors: Dave Coleman

1.2.0 (2014-08-08)

  • Added XXLarge size
  • Added global_scale feature
  • Added hideRobot() functionality
  • Added removeAllCollisionObjects from planning scene monitor
  • Added publishCollisionSceneFromFile function
  • Formatting
  • Contributors: Dave Coleman

1.1.0 (2014-07-31)

  • Bug fixes
  • Fixed convertPoint32ToPose
  • Added scale to publishText
  • New publishPolygon, publishMarker, convertPose, convertPointToPose, and convertPoint32 functions
  • New deleteAllMarkers, publishPath, publishSpheres, and convertPoseToPoint functions
  • Added getCollisionWall
  • Made lines darker
  • Added reset marker feature
  • Namespaces for publishSphere
  • New publishTrajectory function
  • Merging features from OMPL viewer
  • Refactored functions, new robot_model intialization
  • Added more rand functions and made them static
  • Added graph_msgs generated messages dependence so it waits for it to be compiled
  • Updated README
  • Contributors: Dave Coleman, Sammy Pfeiffer

1.0.1 (2014-05-30)

  • Updated README
  • Indigo support
  • Fix for strict cppcheck and g++ warnings/errors
  • Compatibilty fix for Eigen package in ROS Indigo
  • Fix uninitialized
  • Fix functions with no return statement and other cppcheck errors
  • Contributors: Bence Magyar, Dave Coleman, Jordi Pages

1.0.0 (2014-05-05)

  • Enabled dual arm manipulation
  • Removed notions of a global planning group, ee group name, or ee parent link.
  • Changed functionality of loadEEMarker
  • Added new print function
  • Made getPlanningSceneMonitor() private function
  • Renamed loadPathPub()
  • Added tool for visualizing unmangled stack trace
  • Created function for publishing non-animated grasps
  • Created new publishGraph function. Renamed publishCollisionTree to publishCollisionGraph
  • Created functions for loading publishers with a delay
  • Removed old method of removing all collision objects
  • Created better testing functionality
  • Changed return type from void to bool for many functions
  • Changed way trajectory is timed
  • Created new publishIKSolutions() function for grasp poses, etc
  • Added new MoveIt robot state functionality
  • Added visualize grasp functionality
  • Removed unnecessary run dependencies
  • Updated README

0.2.0 (2014-04-11)

  • Improved header comments are re-ordered functions into groups
  • Started to create new trajectory point publisher
  • Added getBaseLink function
  • Added dependency on graph_msgs
  • Added new collision cylinder functionality
  • Created example code in README
  • Renamed visualization to visual keyword
  • Updated README

0.1.0 (2014-04-04)

  • Split moveit_visual_tools from its original usage within block_grasp_generator package

Wiki Tutorials

See ROS Wiki Tutorials for more details.

Source Tutorials

Not currently indexed.

Launch files

Messages

No message files found.

Services

No service files found

Plugins

No plugins found.

Recent questions tagged moveit_visual_tools at Robotics Stack Exchange