void DemoApplication::moveHome()
{
//ROS_ERROR_STREAM("Task '"<<__FUNCTION__ <<"' is incomplete. Exiting"); exit(-1);
// creating move group interface for planning simple moves
move_group_interface::MoveGroup move_group(config_.group_name);
move_group.setPlannerId(PLANNER_ID);
// setting home position as target
if(!move_group.setNamedTarget(HOME_POSITION_NAME))
{
ROS_ERROR_STREAM("Failed to set home '"<<HOME_POSITION_NAME<<"' position");
exit(-1);
}
// moving home
/* Fill Code:
* Goal:
* - Call the move_group.move() and save the returned flag into the "result" variable
* - Verify that the robot reached the goal.
* Hint:
* - The "result.val" and "result.SUCCESS" flags can be used to verify that the move was completed
* -
*/
//moveit_msgs::MoveItErrorCodes result /* [ COMPLETE HERE ]: = move_group.??() ;*/;
//if(false /* [ COMPLETE HERE ]: result.?? != result.?? */)
moveit_msgs::MoveItErrorCodes result = move_group.move();
if(result.val != result.SUCCESS)
{
ROS_ERROR_STREAM("Failed to move to "<<HOME_POSITION_NAME<<" position");
exit(-1);
}
else
{
ROS_INFO_STREAM("Robot reached home position");
}
ROS_INFO_STREAM("Task '"<<__FUNCTION__<<"' completed");
}
/*
* This function will iterate the chain_rec and do the following modifications:
* 1. record all the groups in the chains.
* 2. for every group, do:
* 1) modify Sub Alloc Slot in extent block/inodes accordingly.
* 2) change the GROUP_PARENT according to its future owner.
* 3) link the group to the new slot files.
*/
static errcode_t move_chain_rec(ocfs2_filesys *fs, struct relink_ctxt *ctxt)
{
errcode_t ret = 0;
int i, start, end = 1;
uint64_t blkno, gd_blkno = ctxt->cr->c_blkno;
struct ocfs2_group_desc *gd = NULL;
struct moved_group *group = NULL, *group_head = NULL;
if (gd_blkno == 0)
goto bail;
/* Record the group in the relink_ctxt.
*
* We record the group in a reverse order, so the first group
* will be at the end of the group list. This is useful for
* fsck.ocfs2 when any error happens during the move of groups
* and we can safely move the group also.
*/
while (gd_blkno) {
ret = ocfs2_malloc0(sizeof(struct moved_group), &group);
if (ret)
goto bail;
memset(group, 0, sizeof(struct moved_group));
/* We insert the group first in case of any further error
* will not cause memory leak.
*/
group->next = group_head;
group_head = group;
ret = ocfs2_malloc_block(fs->fs_io, &group->gd_buf);
if (ret)
goto bail;
ret = ocfs2_read_group_desc(fs, gd_blkno, group->gd_buf);
if (ret)
goto bail;
group->blkno = gd_blkno;
gd = (struct ocfs2_group_desc *)group->gd_buf;
gd_blkno = gd->bg_next_group;
}
group = group_head;
while (group) {
gd = (struct ocfs2_group_desc *)group->gd_buf;
end = 1;
/* Modify the "Sub Alloc Slot" in the extent block/inodes. */
while (end < gd->bg_bits) {
start = ocfs2_find_next_bit_set(gd->bg_bitmap,
gd->bg_bits, end);
if (start >= gd->bg_bits)
break;
end = ocfs2_find_next_bit_clear(gd->bg_bitmap,
gd->bg_bits, start);
for (i = start; i < end; i++) {
blkno = group->blkno + i;
ret = change_sub_alloc_slot(fs, blkno, ctxt);
if (ret)
goto bail;
}
}
/* move the group to the new slots. */
ret = move_group(fs, ctxt, group);
if (ret)
goto bail;
group = group->next;
}
bail:
group = group_head;
while (group) {
group_head = group->next;
if (group->gd_buf)
ocfs2_free(&group->gd_buf);
ocfs2_free(&group);
group = group_head;
}
return ret;
}
