/**
* Copy constructor.
*
* @param other the AdaptSRNParametersOperator object to copy.
*/
AdaptSRNParametersOperator::AdaptSRNParametersOperator(const AdaptSRNParametersOperator &other)
: NeuralNetworkManipulationOperator(other)
{
mChangeProbability = dynamic_cast<NormalizedDoubleValue*>(
getParameter("ProbabilityForChange"));
mGlobalSettings = dynamic_cast<BoolValue*>(getParameter("ApplyGlobally"));
fetchParameters("Alpha");
fetchParameters("Beta");
fetchParameters("Gamma");
fetchParameters("Delta");
fetchParameters("AStar");
}
Republish()
{
ros::NodeHandle nh;
// getting parameters
fetchParameters();
// setting up ros subscribers
point_cloud2_subs_ = nh.subscribe(POINT_CLOUD2_TOPIC_IN,1,&Republish::pointCloud2SubsCallback,this);
// setting up ros publishers
point_cloud2_publ_ = nh.advertise<sensor_msgs::PointCloud2>(POINT_CLOUD2_TOPIC_OUT,1);
}
void SpherePickingRobotNavigator::setup()
{
ros::NodeHandle nh;
std::string nodeName = ros::this_node::getName();
ROS_INFO_STREAM(NODE_NAME<<": Loading ros parameters");
// getting ros parametets
fetchParameters(NAVIGATOR_NAMESPACE);
ROS_INFO_STREAM(NODE_NAME<<": Setting up execution Monitors");
// setting up execution monitors
{
joint_state_recorder_.reset(new JointStateTrajectoryRecorder(joint_states_topic_));
arm_controller_handler_.reset(new FollowJointTrajectoryControllerHandler(arm_group_name_,trajectory_action_service_));
gripper_controller_handler_.reset(new GraspPoseControllerHandler(gripper_group_name_,grasp_action_service_));
trajectory_execution_monitor_.addTrajectoryRecorder(joint_state_recorder_);
trajectory_execution_monitor_.addTrajectoryControllerHandler(arm_controller_handler_);
trajectory_execution_monitor_.addTrajectoryControllerHandler(gripper_controller_handler_);
}
ROS_INFO_STREAM(NODE_NAME<<": Setting up Service Clients");
// setting up service clients, this is configuration specific
{
// segmentation
seg_srv_ = nh.serviceClient<tabletop_object_detector::TabletopSegmentation>(segmentation_service_, true);
// path and grasp planning
grasp_planning_client = nh.serviceClient<object_manipulation_msgs::GraspPlanning>(grasp_planning_service_, true);
planning_service_client_ = nh.serviceClient<arm_navigation_msgs::GetMotionPlan>(path_planner_service_);
// arm trajectory filter service
trajectory_filter_service_client_ = nh.serviceClient<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>(trajectory_filter_service_);
ROS_INFO_STREAM(NODE_NAME<<": Waiting for trajectory filter service");
trajectory_filter_service_client_.waitForExistence();
ROS_INFO_STREAM(NODE_NAME<<": Trajectory filter service connected");
// planing scene
ROS_INFO_STREAM(NODE_NAME <<": Waiting for " + planning_scene_service_ + " service");
ros::service::waitForService(planning_scene_service_);
set_planning_scene_diff_client_ = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(planning_scene_service_);
}
// will use grasp execution client to request pre-grasp action since the default gripper controller handler
// ignores this step.
ROS_INFO_STREAM(NODE_NAME << ": Setting up Service Action Clients");
{
grasp_exec_action_client_ =
boost::make_shared< GraspActionServerClient >(grasp_action_service_,true);
while(!grasp_exec_action_client_->waitForServer(ros::Duration(0.5)))
{
ROS_INFO_STREAM(NODE_NAME << "Waiting for action service "<< grasp_action_service_);
}
ROS_INFO_STREAM(NODE_NAME<<" : Connected to action service "<<grasp_action_service_);
}
ROS_INFO_STREAM(NODE_NAME<<": Setting up ros publishers");
{
// setting up ros publishers
marker_publisher_ = nh.advertise<visualization_msgs::Marker> (VISUALIZATION_TOPIC, 128);
attached_object_publisher_ = nh.advertise<arm_navigation_msgs::AttachedCollisionObject> ("attached_collision_object_alternate", 1);
// setting up timer obj
marker_pub_timer_ = nh.createTimer(ros::Duration(0.4f),&SpherePickingRobotNavigator::callbackPublishMarkers,this);
ROS_INFO_STREAM(NODE_NAME<<": Setting up dynamic libraries");
// others
//grasp_tester_ = GraspTesterPtr(new object_manipulator::GraspTesterFast(&cm_, ik_plugin_name_));
grasp_tester_ = GraspTesterPtr(new GraspSequenceValidator(&cm_, ik_plugin_name_));
place_tester_ = PlaceSequencePtr(new PlaceSequenceValidator(&cm_, ik_plugin_name_));
// trajectory callbacks
trajectories_finished_function_ = boost::bind(&SpherePickingRobotNavigator::trajectoryFinishedCallback, this, true,_1);
grasp_action_finished_function_ = boost::bind(&SpherePickingRobotNavigator::trajectoryFinishedCallback, this, false,_1);
}
ROS_INFO_STREAM(NODE_NAME<<": Setting up published markers");
{
visualization_msgs::Marker marker;
marker.header.frame_id = cm_.getWorldFrameId();
marker.ns = NODE_NAME;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::DELETE;
tf::poseTFToMsg(tf::Transform::getIdentity(),marker.pose);
// adding marker to map
addMarker(MARKER_SEGMENTED_OBJECT,marker);
addMarker(MARKER_ATTACHED_OBJECT,marker);
}
ROS_INFO_STREAM(NODE_NAME<<": Finished setup");
}
