Wrong goal Visualization

[ChamzasKonstantinos]

I think there is something wrong in the goal visulization code, regarding the rotations. The Wrong goal visualization can be seen in the first figure. This is happening after I merged #57 to my code

One arrow is by directly using addArrowMarker, and the other is using the goal Visulization

An IK solution is shown to verify the the addArrowMarker works.

The code to reproduce this is the following

`

auto base_to_object =
tf2::transformToEigen(lookup("base_link", "vicon_object_server/" + object));

auto epose = base_to_object * rpose;

// Region properties
auto tol = Eigen::Vector3d{0.001, 0.001, 0.001};
auto region = robowflex::Geometry::makeBox(0.01, 0.01, 0.01);
auto rot = Eigen::Quaterniond();

// Visualizing,
robowflex::MotionRequestBuilder vrequest(planner_, GROUP);

// //  // wrist_roll_link is the default end effector
vrequest.setGoalRegion("wrist_roll_link", "base_link", epose, region, rot, tol);

rviz.removeMarker("goal");
rviz.removeMarker("test");

rviz.addGoalMarker("test", vrequest);
rviz.addArrowMarker("goal", "base_link", epose, Eigen::Vector4d{0.2, 0.3, 0.7, 1.0},
                    Eigen::Vector3d{0.1, 0.008, 0.003});
rviz.updateMarkers();

waitForUserConfirmation("Find the IK solution");

// Pulling the current scene
arm_with_torso_.pullScene(scene_);

// Finding the an IK solution
if (!fetch_->setFromIKCollisionAware(scene_, GROUP, region, epose, rot, tol))
{
    ROS_WARN("No IK solution found for %s aborting ...", object.c_str());
    return false;
}

rviz.visualizeCurrentState();
request.setGoalConfiguration(fetch_->getScratchState());

`

[zkingston]

Was this fixed by your recent pull?

[ChamzasKonstantinos]

To tell you the truth, I don't know I stopped using addGoalMarker and started using addArrowMarker. I can check it sometime later

[zkingston]

Can you follow up on this?

[ChamzasKonstantinos]

Argh, It is actually a bit hard for me to reproduce right now, since I stopped using the pose goals, and I use only joint goals and addArrowMarker. Maybe we can close for now. It only has to do with an rviz helper function it's not that important anyway

[ChamzasKonstantinos]

I tested this again, and it still not working as expecting the goal is orientation is wrong. There must be something wrong in this function, but I don't know what
https://github.com/KavrakiLab/robowflex/blob/master/robowflex_library/src/io/visualization.cpp#L259