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));
robot.setUserCallback(userCallback);
help();
ros::spin();
delete g_planning_scene;
delete g_marker_array_publisher;;
ros::shutdown();
return 0;
}
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;
}
