void displayStates(robot_state::RobotState& start_state,
robot_state::RobotState& goal_state, ros::NodeHandle& node_handle,
robot_model::RobotModelPtr& robot_model)
{
static ros::Publisher start_state_display_publisher = node_handle.advertise<
moveit_msgs::DisplayRobotState>("/move_itomp/display_start_state",
1, true);
static ros::Publisher goal_state_display_publisher = node_handle.advertise<
moveit_msgs::DisplayRobotState>("/move_itomp/display_goal_state", 1,
true);
int num_variables = start_state.getVariableNames().size();
moveit_msgs::DisplayRobotState disp_start_state;
disp_start_state.state.joint_state.header.frame_id =
robot_model->getModelFrame();
disp_start_state.state.joint_state.name = start_state.getVariableNames();
disp_start_state.state.joint_state.position.resize(num_variables);
memcpy(&disp_start_state.state.joint_state.position[0],
start_state.getVariablePositions(), sizeof(double) * num_variables);
disp_start_state.highlight_links.clear();
const std::vector<std::string>& link_model_names =
robot_model->getLinkModelNames();
for (int i = 0; i < link_model_names.size(); ++i)
{
std_msgs::ColorRGBA color;
color.a = 0.5;
color.r = 0.0;
color.g = 1.0;
color.b = 0.5;
moveit_msgs::ObjectColor obj_color;
obj_color.id = link_model_names[i];
obj_color.color = color;
disp_start_state.highlight_links.push_back(obj_color);
}
start_state_display_publisher.publish(disp_start_state);
moveit_msgs::DisplayRobotState disp_goal_state;
disp_goal_state.state.joint_state.header.frame_id =
robot_model->getModelFrame();
disp_goal_state.state.joint_state.name = goal_state.getVariableNames();
disp_goal_state.state.joint_state.position.resize(num_variables);
memcpy(&disp_goal_state.state.joint_state.position[0],
goal_state.getVariablePositions(), sizeof(double) * num_variables);
disp_goal_state.highlight_links.clear();
for (int i = 0; i < link_model_names.size(); ++i)
{
std_msgs::ColorRGBA color;
color.a = 0.5;
color.r = 0.0;
color.g = 0.5;
color.b = 1.0;
moveit_msgs::ObjectColor obj_color;
obj_color.id = link_model_names[i];
obj_color.color = color;
disp_goal_state.highlight_links.push_back(obj_color);
}
goal_state_display_publisher.publish(disp_goal_state);
}
int main(int argc, char *argv[])
{
ros::init (argc, argv, "kinematic_model_tutorial");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
/* Load the robot model */
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
/* Get a shared pointer to the model */
kinematic_model = robot_model_loader.getModel();
/* Get and print the name of the coordinate frame in
* which the transforms for this model are computed*/
ROS_INFO("Model frame: %s", kinematic_model->getModelFrame().c_str());
/* Create a kinematic state - this represents the configuration for
* the robot represented by kinematic_model */
kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
/* Set all joints in this state to their default values */
kinematic_state->setToDefaultValues();
joint_model_group = kinematic_model->getJointModelGroup("arm_1");
moveitIKPseudoInverseSolver();
ROS_INFO_STREAM("Test complete.");
return -1;
}
void renderEnvironment(const std::string& environment_file,
robot_model::RobotModelPtr& robot_model, const std::string& ns,
std_msgs::ColorRGBA& color)
{
return;
ros::NodeHandle node_handle;
ros::Publisher vis_marker_array_publisher_ = node_handle.advertise<
visualization_msgs::MarkerArray>(
"move_itomp/visualization_marker_array", 10);
visualization_msgs::MarkerArray ma;
visualization_msgs::Marker msg;
msg.header.frame_id = robot_model->getModelFrame();
msg.header.stamp = ros::Time::now();
msg.ns = ns;
msg.type = visualization_msgs::Marker::MESH_RESOURCE;
msg.action = visualization_msgs::Marker::ADD;
msg.scale.x = 1.0;
msg.scale.y = 1.0;
msg.scale.z = 1.0;
msg.id = 0;
msg.pose.position.x = 0.0;
msg.pose.position.y = 0.0;
msg.pose.position.z = 0.0;
msg.pose.orientation.x = 0.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
msg.pose.orientation.w = 1.0;
msg.color = color;
/*
msg.color.a = 1.0;
msg.color.r = 0.5;
msg.color.g = 0.5;
msg.color.b = 0.5;
*/
msg.mesh_resource = environment_file;
ma.markers.push_back(msg);
vis_marker_array_publisher_.publish(ma);
}
void renderStaticScene(ros::NodeHandle& node_handle,
planning_scene::PlanningScenePtr& planning_scene,
robot_model::RobotModelPtr& robot_model)
{
std::string environment_file;
std::vector<double> environment_position;
static ros::Publisher vis_marker_array_publisher_ = node_handle.advertise<
visualization_msgs::MarkerArray>("visualization_marker_array", 10);
ros::WallDuration(1.0).sleep();
node_handle.param<std::string>("/itomp_planner/environment_model",
environment_file, "");
if (!environment_file.empty())
{
environment_position.resize(3, 0);
if (node_handle.hasParam("/itomp_planner/environment_model_position"))
{
XmlRpc::XmlRpcValue segment;
node_handle.getParam("/itomp_planner/environment_model_position",
segment);
if (segment.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
int size = segment.size();
for (int i = 0; i < size; ++i)
{
double value = segment[i];
environment_position[i] = value;
}
}
}
visualization_msgs::MarkerArray ma;
visualization_msgs::Marker msg;
msg.header.frame_id = robot_model->getModelFrame();
msg.header.stamp = ros::Time::now();
msg.ns = "environment";
msg.type = visualization_msgs::Marker::MESH_RESOURCE;
msg.action = visualization_msgs::Marker::ADD;
msg.scale.x = 1.0;
msg.scale.y = 1.0;
msg.scale.z = 1.0;
msg.id = 0;
msg.pose.position.x = environment_position[0];
msg.pose.position.y = environment_position[1];
msg.pose.position.z = environment_position[2];
ROS_INFO(
"Env render pos : (%f %f %f)", environment_position[0], environment_position[1], environment_position[2]);
msg.pose.orientation.x = 0.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
msg.pose.orientation.w = 1.0;
msg.color.a = 1.0;
msg.color.r = 0.5;
msg.color.g = 0.5;
msg.color.b = 0.5;
msg.mesh_resource = environment_file;
ma.markers.push_back(msg);
msg.ns = "environment2";
msg.type = visualization_msgs::Marker::CUBE;
msg.id = 0;
msg.pose.position.x = 0;
msg.pose.position.y = 6.0;
msg.pose.position.z = -0.45;
msg.scale.x = 2.0;
msg.scale.y = 1.0;
msg.scale.z = 1.0;
ma.markers.push_back(msg);
ros::WallDuration(1.0).sleep();
vis_marker_array_publisher_.publish(ma);
}
}
void loadStaticScene(ros::NodeHandle& node_handle,
planning_scene::PlanningScenePtr& planning_scene,
robot_model::RobotModelPtr& robot_model)
{
std::string environment_file;
std::vector<double> environment_position;
node_handle.param<std::string>("/itomp_planner/environment_model",
environment_file, "");
if (!environment_file.empty())
{
environment_position.resize(3, 0);
if (node_handle.hasParam("/itomp_planner/environment_model_position"))
{
XmlRpc::XmlRpcValue segment;
node_handle.getParam("/itomp_planner/environment_model_position",
segment);
if (segment.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
int size = segment.size();
for (int i = 0; i < size; ++i)
{
double value = segment[i];
environment_position[i] = value;
}
}
}
// Collision object
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = robot_model->getModelFrame();
collision_object.id = "environment";
geometry_msgs::Pose pose;
pose.position.x = environment_position[0];
pose.position.y = environment_position[1];
pose.position.z = environment_position[2];
ROS_INFO(
"Env col pos : (%f %f %f)", environment_position[0], environment_position[1], environment_position[2]);
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
pose.orientation.z = 0.0;
pose.orientation.w = 1.0;
shapes::Mesh* shape = shapes::createMeshFromResource(environment_file);
shapes::ShapeMsg mesh_msg;
shapes::constructMsgFromShape(shape, mesh_msg);
shape_msgs::Mesh mesh = boost::get<shape_msgs::Mesh>(mesh_msg);
collision_object.meshes.push_back(mesh);
collision_object.mesh_poses.push_back(pose);
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 2.0;
primitive.dimensions[1] = 1.0;
primitive.dimensions[2] = 1.0;
pose.position.x = 0;
pose.position.y = 6.0;
pose.position.z = -0.45;
collision_object.primitive_poses.push_back(pose);
collision_object.primitives.push_back(primitive);
collision_object.operation = collision_object.ADD;
moveit_msgs::PlanningScene planning_scene_msg;
planning_scene_msg.world.collision_objects.push_back(collision_object);
planning_scene_msg.is_diff = true;
planning_scene->setPlanningSceneDiffMsg(planning_scene_msg);
}
}