I spent the past two weeks coding up the movement interface, a model plugin that will allow a user to specify a series of points and move the model between those points in Gazebo.
Figuring out how to do this was a little tricker than I anticipated, since it involves using PoseAnimation, a library that isn’t well-documented, apart from a tutorial that just points to some short example code.
To animate a model in Gazebo, you need to create an animation object and populate it with a set of keyFrames. Each frame specifies translation and rotation of the model relative to the last frame. To begin, the animation gets a an initial frame with a translation of zero (meaning the model’s initial position). For the model movement plugin, the rest of the frames are generated by calculating the coordinate translation from the model’s initial position to goal position, then creating a frame for each second of the animation, dividing the translation amount by the number of frames and assigning that translation value to each frame, thus creating a constant velocity. The animation length is somewhat arbitrary, but for now is based on the euclidean distance from start to end point, one second per unit. [Note: no rotation in the animation for now!]
To animate movement between a sequence of points, this frame creation process is repeated for each consecutive pair of points, A->B, B->C, etc. Unfortunately, since there can only be one animation per model (as far as I can tell), stringing together these subsets of frames is a little ugly, and involves passing around current translation values. I’ll see if I can clean-up the code a little in the coming weeks.
This movement plugin has two intended usages, each with a different method of getting the required parameters.
One use case is as a standalone plugin, for gazebo users to do it as they will. For this use case, Jose and I decided that the easiest way to pass in parameters is via the sdf file. Under the <plugin> tag, users need to set <path>, a string of positions, and <n_points>, the number of points in the path. Using the sdf pointer passed to the load() function, these parameters can be read within the plugin.
The other use case is, of course, as a part of the path planner plugin — once the planner algorithm generates a valid path, the path planner plugin needs to be able to call the movement interface to actually move the model along the path. For the planner plugin to communicate with the movement interface, we use the gazebo transport system, which uses Google Protocol Buffers to transmit messages between processes by subscribing and publishing to known channels. To read parameters this way, the movement plugin creates a transport node and subscribes to the “~/pose_animation” channel. Upon receiving a message, a callback function is triggered, which parses the messages into the series of poses that specify the path.
The movement interface is pretty much functional (minus a bug with a test path publisher I that made). There’s definitely some clean-up that I could do, but in the interest of having a complete path planner plugin, I’m moving on for now. Since the collision map generator is basically done, the only major parts left are the planner algorithm, and the overall plugin which will actually stitch all these pieces together.