void userCallback(InteractiveRobot& robot)
{
// move the world geometry in the collision world
Eigen::Affine3d world_cube_pose;
double world_cube_size;
robot.getWorldGeometry(world_cube_pose, world_cube_size);
g_planning_scene->getWorldNonConst()->moveShapeInObject("world_cube", g_world_cube_shape, world_cube_pose);
// prepare to check collisions
collision_detection::CollisionRequest c_req;
collision_detection::CollisionResult c_res;
c_req.group_name = "right_gripper";
//c_req.group_name = "right_arm";
c_req.contacts = true;
c_req.max_contacts = 100;
c_req.max_contacts_per_pair = 5;
c_req.verbose = true;
// check for collisions between robot and itself or the world
g_planning_scene->checkCollision(c_req, c_res, *robot.robotState());
// display results of the collision check
if (c_res.collision)
{
ROS_INFO("COLLIDING contact_point_count=%d",(int)c_res.contact_count);
// get the contact points and display them as markers
if (c_res.contact_count > 0)
{
std_msgs::ColorRGBA color;
color.r = 1.0;
color.g = 0.0;
color.b = 1.0;
color.a = 0.5;
visualization_msgs::MarkerArray markers;
collision_detection::getCollisionMarkersFromContacts(markers,
"base_footprint",
c_res.contacts,
color,
ros::Duration(), // remain until deleted
0.01); // radius
publishMarkers(markers);
}
}
else
{
ROS_INFO("Not colliding");
// delete the old collision point markers
visualization_msgs::MarkerArray empty_marker_array;
publishMarkers(empty_marker_array);
}
}
void userCallback(InteractiveRobot& robot)
{
ROS_INFO_STREAM("Robot position: " <<
PoseString(robot.robotState()->getLinkState("r_wrist_roll_link")->getGlobalLinkTransform()));
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "acorn_play");
ros::NodeHandle nh;
InteractiveRobot robot;
// create a PlanningScene
g_planning_scene = new planning_scene::PlanningScene(robot.robotModel());
// Add the world geometry to the PlanningScene's collision detection world
Eigen::Affine3d world_cube_pose;
double world_cube_size;
robot.getWorldGeometry(world_cube_pose, world_cube_size);
g_world_cube_shape.reset(new shapes::Box(world_cube_size, world_cube_size, world_cube_size));
g_planning_scene->getWorldNonConst()->addToObject("world_cube", g_world_cube_shape, world_cube_pose);
// Create a marker array publisher for publishing contact points
g_marker_array_publisher = new ros::Publisher(nh.advertise<visualization_msgs::MarkerArray>("interactive_robot_marray",100));
// Attach a bar object to the right gripper
robot_state::LinkState *link = robot.robotState()->getLinkState("r_gripper_palm_link");
//robot_state::LinkState *link = robot.robotState()->getLinkState("r_gripper_r_finger_link");
//robot_state::LinkState *link = robot.robotState()->getLinkState("r_wrist_roll_link");
//
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
shapes::ShapePtr bar_shape;
bar_shape.reset(new shapes::Cylinder(0.02,1.0));
//bar_shape.reset(new shapes::Box(0.02,.02,1));
shapes.push_back(bar_shape);
poses.push_back(Eigen::Affine3d(Eigen::Translation3d(0.12,0,0)));
const robot_model::JointModelGroup* r_gripper_group = robot.robotModel()->getJointModelGroup("right_gripper");
const std::vector<std::string>& touch_links = r_gripper_group->getLinkModelNames();
robot.robotState()->attachBody("bar",
shapes,
poses,
touch_links,
"r_gripper_palm_link");
robot.setUserCallback(userCallback);
help();
ros::spin();
delete g_planning_scene;
delete g_marker_array_publisher;;
ros::shutdown();
return 0;
}