void LineStripMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
{
    ROS_ASSERT(new_message->type == visualization_msgs::Marker::LINE_STRIP);

    if (!lines_)
    {
        lines_ = new BillboardLine(context_->getSceneManager(), scene_node_);
    }

    Ogre::Vector3 pos, scale;
    Ogre::Quaternion orient;
    transform(new_message, pos, orient, scale);

    setPosition(pos);
    setOrientation(orient);
    lines_->setScale(scale);
    lines_->setColor(new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a);

    lines_->clear();
    if (new_message->points.empty())
    {
        return;
    }

    lines_->setLineWidth(new_message->scale.x);
    lines_->setMaxPointsPerLine(new_message->points.size());

    bool has_per_point_color = new_message->colors.size() == new_message->points.size();

    size_t i = 0;
    std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
    std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
    for ( ; it != end; ++it, ++i )
    {
        const geometry_msgs::Point& p = *it;

        Ogre::Vector3 v( p.x, p.y, p.z );

        Ogre::ColourValue c;
        if (has_per_point_color)
        {
            const std_msgs::ColorRGBA& color = new_message->colors[i];
            c.r = color.r;
            c.g = color.g;
            c.b = color.b;
            c.a = color.a * new_message->color.a;
        }
        else
        {
            c.r = new_message->color.r;
            c.g = new_message->color.g;
            c.b = new_message->color.b;
            c.a = new_message->color.a;
        }

        lines_->addPoint( v, c );
    }
}
void StompOptimizationTask::setFeatureWeights(const std::vector<double>& weights)
{
  ROS_ASSERT((int)weights.size() == num_split_features_);
  feature_weights_ = Eigen::VectorXd::Zero(weights.size());
  for (unsigned int i=0; i<weights.size(); ++i)
  {
    feature_weights_(i) = weights[i];
  }
}
	ServoStatePublisher() :
		nh()
	{
		ros::NodeHandle priv_nh("~");

		XmlRpc::XmlRpcValue param_dict;
		priv_nh.getParam("", param_dict);

		ROS_ASSERT(param_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct);

		urdf::Model model;
		model.initParam("robot_description");
		ROS_INFO("SSP: URDF robot: %s", model.getName().c_str());

		for (auto &pair : param_dict) {
			ROS_DEBUG("SSP: Loading joint: %s", pair.first.c_str());

			// inefficient, but easier to program
			ros::NodeHandle pnh(priv_nh, pair.first);

			bool rc_rev;
			int rc_channel, rc_min, rc_max, rc_trim, rc_dz;

			if (!pnh.getParam("rc_channel", rc_channel)) {
				ROS_ERROR("SSP: '%s' should provice rc_channel", pair.first.c_str());
				continue;
			}

			pnh.param("rc_min", rc_min, 1000);
			pnh.param("rc_max", rc_max, 2000);
			if (!pnh.getParam("rc_trim", rc_trim)) {
				rc_trim = rc_min + (rc_max - rc_min) / 2;
			}

			pnh.param("rc_dz", rc_dz, 0);
			pnh.param("rc_rev", rc_rev, false);

			auto joint = model.getJoint(pair.first);
			if (!joint) {
				ROS_ERROR("SSP: URDF: there no joint '%s'", pair.first.c_str());
				continue;
			}
			if (!joint->limits) {
				ROS_ERROR("SSP: URDF: joint '%s' should provide <limit>", pair.first.c_str());
				continue;
			}

			double lower = joint->limits->lower;
			double upper = joint->limits->upper;

			servos.emplace_back(pair.first, lower, upper, rc_channel, rc_min, rc_max, rc_trim, rc_dz, rc_rev);
			ROS_INFO("SSP: joint '%s' (RC%d) loaded", pair.first.c_str(), rc_channel);
		}

		rc_out_sub = nh.subscribe("rc_out", 10, &ServoStatePublisher::rc_out_cb, this);
		joint_states_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
	}
Exemple #4
0
void VoxelLayer::matchSize()
{
  ObstacleLayer::matchSize();
  voxel_grid_.resize(size_x_, size_y_, size_z_);
  ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
  if(clear_old_){
    locations_utime.resize(voxel_grid_.sizeX() * voxel_grid_.sizeY());
  }
}
void CLegGrid::filterStates()
{

    ROS_ASSERT(cmeas_.size() == cstate_.size());

    std::vector<PolarPose> fstate, fmeas;
    PolarPose nstate, s1, s2, nmeas, m1, m2;

    /* check for close states */
    if(cstate_.size() < 2)
    {
        fstate = cstate_;
        fmeas = cmeas_;
    }
    else
    {
        if(!fstate.empty()) fstate.clear();
        fstate.push_back(cstate_.at(0));

        if(!fmeas.empty()) fmeas.clear();
        fmeas.push_back(cmeas_.at(0));

        for(uint8_t i = 1; i < cstate_.size(); i++)
        {
            s1 = cstate_.at(i);
            s2 = fstate.back();

            m1 = cmeas_.at(i);
            m2 = fmeas.back();

            if(s1.distance(s2) < 1.0)
            {
                fstate.pop_back();
                nstate.range = (s1.range + s2.range) / 2;
                nstate.angle = (s1.angle + s2.angle) / 2;
                nstate.setZeroVar();
                fstate.push_back(nstate);

                fmeas.pop_back();
                nmeas.range = (m1.range + m2.range) / 2;
                nmeas.angle = (m1.angle + m2.angle) / 2;
                fmeas.push_back(nstate);

            } else
            {
                fstate.push_back(s1);
                fmeas.push_back(m1);
            }
        }
    }

    if(!cstate_.empty()) cstate_.clear();
    cstate_ = fstate;

    if(!cmeas_.empty()) cmeas_.clear();
    cmeas_ = fmeas;
}
void ForwardKinematics::forwardKinematics(const std::vector<double>& joint_positions, geometry_msgs::Pose& pose)
{
  ROS_ASSERT(initialized_);
  ROS_ASSERT(int(joint_positions.size()) == chain_.getNumJoints());
  KDL::Frame frame;
  for (int i=0; i<chain_.getNumJoints(); ++i)
  {
    kdl_joint_array_(i) = joint_positions[i];
  }
  chain_.forwardKinematics(kdl_joint_array_, frame);
  pose.position.x = frame.p.x();
  pose.position.y = frame.p.y();
  pose.position.z = frame.p.z();
  frame.M.GetQuaternion(pose.orientation.x,
                        pose.orientation.y,
                        pose.orientation.z,
                        pose.orientation.w);
}
void XMLRPCManager::start()
{
  shutting_down_ = false;
  port_ = 0;
  bind("getPid", getPid);

  bool bound = server_.bindAndListen(0);
  (void) bound;
  ROS_ASSERT(bound);
  port_ = server_.get_port();
  ROS_ASSERT(port_ != 0);

  std::stringstream ss;
  ss << "http://" << network::getHost() << ":" << port_ << "/";
  uri_ = ss.str();

  server_thread_ = boost::thread(boost::bind(&XMLRPCManager::serverThreadFunc, this));
}
bool NC2010TransformationSystem::initOneDimensionalTransformationSystemHelper(std::vector<dmp_lib::NC2010TSPtr>& transformation_systems,
                                                                                XmlRpc::XmlRpcValue transformation_systems_parameters_xml,
                                                                                ros::NodeHandle& node_handle,
                                                                                dmp_lib::TransformationSystem::IntegrationMethod integration_method)
{
  ROS_ASSERT(transformation_systems_parameters_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);
  for (int j = 0; j < transformation_systems_parameters_xml.size(); ++j)
  {
    ROS_DEBUG("Initializing NC2010 transformation system number >%i< from node handle.",j);

    // create transformation system
    dmp_lib::NC2010TSPtr transformation_system(new dmp_lib::NC2010TransformationSystem());

    XmlRpc::XmlRpcValue ts_xml = transformation_systems_parameters_xml[j];
    ROS_ASSERT(ts_xml.hasMember(transformation_system->getVersionString()));
    XmlRpc::XmlRpcValue nc2010_ts_xml = ts_xml[transformation_system->getVersionString()];

    double k_gain = 0;
    if (!usc_utilities::getParam(nc2010_ts_xml, "k_gain", k_gain))
    {
      return false;
    }
    double d_gain = dmp_lib::NC2010TransformationSystem::getDGain(k_gain);

    // create parameters
    dmp_lib::NC2010TSParamPtr parameters(new dmp_lib::NC2010TransformationSystemParameters());

    // initialize parameters
    ROS_VERIFY(parameters->initialize(k_gain, d_gain));

    // initialize base class
    ROS_VERIFY(TransformationSystem::initFromNodeHandle(parameters, ts_xml, node_handle));

    // create state
    dmp_lib::NC2010TSStatePtr state(new dmp_lib::NC2010TransformationSystemState());

    // initialize transformation system with parameters and state
    ROS_VERIFY(transformation_system->initialize(parameters, state, integration_method));
    ROS_VERIFY(transformation_system->setIntegrationMethod(integration_method));

    transformation_systems.push_back(transformation_system);
  }
  return true;
}
bool TransportTCP::initializeSocket()
{
    ROS_ASSERT(sock_ != ROS_INVALID_SOCKET);

    if (!setNonBlocking())
    {
        return false;
    }

    setKeepAlive(s_use_keepalive_, 60, 10, 9);

    // connect() will set cached_remote_host_ because it already has the host/port available
    if (cached_remote_host_.empty())
    {
        if (is_server_)
        {
            cached_remote_host_ = "TCPServer Socket";
        }
        else
        {
            std::stringstream ss;
            ss << getClientURI() << " on socket " << sock_;
            cached_remote_host_ = ss.str();
        }
    }

#ifdef ROSCPP_USE_TCP_NODELAY
    setNoDelay(true);
#endif

    ROS_ASSERT(poll_set_ || (flags_ & SYNCHRONOUS));
    if (poll_set_)
    {
        ROS_DEBUG("Adding tcp socket [%d] to pollset", sock_);
        poll_set_->addSocket(sock_, boost::bind(&TransportTCP::socketUpdate, this, _1), shared_from_this());
    }

    if (!(flags_ & SYNCHRONOUS))
    {
        //enableRead();
    }

    return true;
}
	bool ControlTorso::torsoLiftMin(control_robot_msgs::MoveIt::Request &req,
			control_robot_msgs::MoveIt::Response &res)
	{
		std::vector<std::string> torso_joints = torso_group_->getJoints();
		ROS_ASSERT(torso_joints.size() > 0);
		double min_position = torso_group_->getCurrentState().get()->getJointModel(torso_joints[0])->getVariableBounds()[0].min_position_;
		moveit::planning_interface::MoveItErrorCode error_code;
		error_code = moveTorsoJointPosition(min_position);
		return error_code == moveit::planning_interface::MoveItErrorCode::SUCCESS;
	}
void PwmSysfsDriverNode::setDuty(double duty)
{
  ROS_ASSERT(driver_);
  if (duty >= 0 && duty <= 1) {
    ROS_DEBUG("Setting PWM duty cycle to: %f", duty);
    driver_->duty_fraction(duty);
  }
  else
    ROS_WARN("Commanded PWM duty cycle value (%f) invalid, ignoring.", duty);
}
void ServiceServerLink::onConnectionDropped(const ConnectionPtr& conn)
{
  ROS_ASSERT(conn == connection_);
  ROSCPP_LOG_DEBUG("Service client from [%s] for [%s] dropped", conn->getRemoteString().c_str(), service_name_.c_str());

  dropped_ = true;
  clearCalls();

  ServiceManager::instance()->removeServiceServerLink(shared_from_this());
}
void PwmSysfsDriverNode::setPeriod(double period)
{
  ROS_ASSERT(driver_);
  if (period > 0 && period <= UINT_MAX) {
    ROS_DEBUG("Setting PWM period to: %f", period);
    driver_->period(period);
  }
  else
    ROS_WARN("Commanded PWM period value (%f) invalid, ignoring.", period);
}
void MsgTranslationWrapper::updateGripperCommand()
{
/*
             * NOTE: gripper slide rails are always symmetric, but the fingers can be screwed in different
             * positions! The published values account for the distance between the gripper slide rails, not the fingers
             * themselves. Of course if the finger are screwed to the most inner position (i.e. the can close completely),
             * than it is correct.
             */
for (int armIndex = 0; armIndex < static_cast<int> (nodeConfiguration.nodeArmConfigurations.size()); armIndex++)
       {

           ROS_ASSERT(nodeConfiguration.nodeArmConfigurations.size() == gripperTrajectoryMessages.size());  // gripperTrajectoryMessages
           
           gripperTrajectoryMessages[armIndex].joint_names.resize(NumberOfFingers);

           gripperTrajectoryMessages[armIndex].joint_names[0] = "gripper_finger_joint_l";
           gripperTrajectoryMessages[armIndex].joint_names[1] = "gripper_finger_joint_r";
          

             trajectory_msgs::JointTrajectoryPoint  pointGripper ;

             pointGripper.positions.resize(NumberOfFingers);
             pointGripper.velocities.resize(NumberOfFingers);
             pointGripper.effort.resize(NumberOfFingers);
       
            if(static_cast<int>(currentGripperPos.size()) == NumberOfFingers)
              {           
                     for (int i = 0; i < NumberOfFingers; ++i)
                     {
                        pointGripper.positions[i] = currentGripperPos[i] ;
                       
                       }                   
                }else{                       
                        pointGripper.positions[0] =  0.05004093943948895;
                        pointGripper.positions[1] =  0.01294137360159624;                   
               }
             
                        pointGripper.velocities[0] = -0.03175738364753859;
                        pointGripper.velocities[1] = 1.019040908525729466;
                        
              
                        pointGripper.effort[0] = -0.034732168550673986;
                        pointGripper.effort[1] = 0.43880198765294803;
                        

                pointGripper.time_from_start = ros::Duration(0.1);

                gripperTrajectoryMessages[armIndex].points.resize(1);   
                gripperTrajectoryMessages[armIndex].points[0] = pointGripper;//.push_back(point);
                gripperTrajectoryMessages[armIndex].header.stamp = currentTime;  


  }
    
}
Exemple #15
0
int main(int argc, char **argv)
{
	ros::init(argc, argv, "lift_torso");

	ros::NodeHandle nh;
	ros::AsyncSpinner spinner(1);
	spinner.start();

	double position;
	if (argc != 2)
	{
		ROS_ERROR("Usage: rosrun control_robot lift_torso <torso_position>");
		return 1;
	}
	position = atof(argv[1]);
	ROS_INFO("Set torso position to %lf", position);

	moveit::planning_interface::MoveGroup torso_group("torso");
	std::vector<std::string> torso_joints = torso_group.getJoints();
	ROS_ASSERT(torso_joints.size() > 0);
	if (!torso_group.setJointValueTarget(torso_joints[0], position))
	{
		ROS_ERROR("Position out of bounds - not moving torso");
		return 1;
	}
	moveit::planning_interface::MoveItErrorCode error_code;
	ROS_INFO("Lifting torso...");
	error_code = torso_group.move();
	ros::shutdown();
	return error_code == moveit::planning_interface::MoveItErrorCode::SUCCESS;

//	//ros::ServiceClient torso_client = nh.serviceClient<control_robot_msgs::MoveIt>("/control_robot/torso_lift_max");
//	ros::ServiceClient torso_client = nh.serviceClient<control_robot_msgs::MoveItPosition>("/control_robot/torso_lift");
//	control_robot_msgs::MoveItPosition srv;
//	srv.request.position.data = position;
//
//	ROS_INFO("Wait for existence of service: %s", torso_client.getService().c_str());
//	torso_client.waitForExistence();
//
//	if (!torso_client.exists())
//		ROS_ERROR("Client %s does not exist.", torso_client.getService().c_str());
//
//	ROS_INFO("Lifting torso...");
//
//	if (!torso_client.call(srv))
//	{
//		ROS_ERROR("Could not send service message to client: %s", torso_client.getService().c_str());
//		return 1;
//	}
//	ROS_INFO("Service call for torso was successful.");

//	ros::shutdown();
//	return 0;

}
Exemple #16
0
bool SerialServer::callbackSendRecv(shared_serial::SendRecv::Request& req, shared_serial::SendRecv::Response& res)
{
  int socket = lock_.lock(req.socket, req.sock_timeout);
  if (socket < 0) return false;

  // *** Send ***
  unsigned char *buf = new unsigned char[req.send_data.size()];
  for (unsigned int ii=0; ii != req.send_data.size(); ++ii)
    buf[ii] = req.send_data[ii];
    
  int n = serial_port_.port_write(buf, req.send_data.size());
  delete buf;
  
  if (n != (int)req.send_data.size())
  {
    ROS_ERROR("Truncated send, flushing port");
    serial_port_.flush_buffer();
    
    lock_.unlock();
    return false;
  }

  ROS_DEBUG_STREAM("Sent " << n << " bytes");

  // *** Receive ***
  ROS_ASSERT(req.length <= 65536);

  buf = new unsigned char[req.length];
  n = serial_port_.port_read(buf, req.length, (int)req.recv_timeout, (int)((req.recv_timeout-(int)req.recv_timeout)*1000000));
  
  if (n < 0)
  {
    ROS_ERROR("Error while reading serial port");
    delete buf;

    serial_port_.flush_buffer();
    usleep(10);
    serial_port_.flush_buffer();
    
    lock_.unlock();
    return false;
  }
  
  ROS_DEBUG_STREAM("Read " << n << " bytes");
  
  res.recv_data.resize(n);
  for (int ii=0; ii != n; ++ii)
    res.recv_data[ii] = buf[ii];
  delete buf;
  
  res.socket = socket;
    
  lock_.unlock();    
  return true;
}
bool CovariantMovementPrimitive::updateParameters(const std::vector<Eigen::MatrixXd>& updates, const std::vector<Eigen::VectorXd>& time_step_weights)
{
  ROS_ASSERT(int(updates.size()) == num_dimensions_);

  // this takes only the diagonal elements
  /*for (int d=0; d<num_dimensions_; ++d)
    {
        parameters_all_[d].segment(free_vars_start_index_, num_vars_free_) += updates[d].diagonal();
    }*/

  // this averages all the updates
  //double divisor = 1.0 / num_vars_free_;
  double divisor = 1.0;
  for (int d=0; d<num_dimensions_; ++d)
  {
    parameters_all_[d].segment(free_vars_start_index_, num_vars_free_).transpose() +=
        divisor * updates[d].row(0);
  }

  //    for (int d=0; d<num_dimensions_; ++d)
  //    {
  //      double weight = 0.0;
  //      double weight_sum = 0.0;
  //
  //      Eigen::VectorXd update = Eigen::VectorXd::Zero(num_vars_free_);
  //      for (int t=0; t<num_time_steps_; ++t)
  //      {
  //          weight = time_step_weights[d][t];
  //          weight_sum += weight;
  //          update.transpose() += updates[d].row(t) * weight;
  //          //ROS_INFO_STREAM("Update at time " << t << " = " << updates[d].row(t));
  //      }
  //      if (weight_sum <1e-6)
  //        weight_sum = 1e-6;
  //      parameters_all_[d].segment(free_vars_start_index_, num_vars_free_) += (1.0/weight_sum)*update;
  //    }

  // this weights updates by number of time-steps remaining:
  //    for (int d=0; d<num_dimensions_; ++d)
  //    {
  //        double weight=0.0;
  //        double weight_sum=0.0;
  //        Eigen::VectorXd update = Eigen::VectorXd::Zero(num_vars_free_);
  //        for (int t=0; t<num_time_steps_; ++t)
  //        {
  //            weight = double(num_time_steps_ - t);
  //            weight_sum += weight;
  //            update.transpose() += updates[d].row(t) * weight;
  //            //ROS_INFO_STREAM("Update at time " << t << " = " << updates[d].row(t));
  //        }
  //        parameters_all_[d].segment(free_vars_start_index_, num_vars_free_) += (1.0/weight_sum)*update;
  //    }

  return true;
}
  SegmentationVisualizer::SegmentationVisualizer(const std::vector<KinectCloud::Ptr>& original,
                                                 const std::vector<KinectCloud::Ptr>& segmented) :
    quit_at_end_(false),
    vis_("Segmentation"),
    original_(original),
    segmented_(segmented),
    idx_(0),
    stepping_(false),
    recording_(false)
  {
    ROS_ASSERT(!original_.empty());
    ROS_ASSERT(original_.size() == segmented_.size());
  
    vis_.registerKeyboardCallback(&SegmentationVisualizer::keyboardEventOccurred, *this);
    vis_.setBackgroundColor(255, 255, 255);
    PointCloudColorHandlerCustom<PointXYZRGB> single_color(original_[0], 0, 0, 0);
    vis_.addPointCloud(original_[0], single_color, "original");
    vis_.addPointCloud(segmented_[0], "segmented");
    vis_.setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 1, "original");
    vis_.setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 4, "segmented");
    
    // -- Hardcode the camera for the Kinect.
    ROS_WARN("[SegmentationVisualizer] PCLVisualizer's camera param has been removed.  Initial camera pose needs to be set somehow...");
    // vis_.camera_.clip[0] = 0.00387244;
    // vis_.camera_.clip[1] = 3.87244;
    // vis_.camera_.focal[0] = -0.160878;
    // vis_.camera_.focal[1] = -0.0444743;
    // vis_.camera_.focal[2] = 1.281;
    // vis_.camera_.pos[0] = 0.0402195;
    // vis_.camera_.pos[1] = 0.0111186;
    // vis_.camera_.pos[2] = -1.7;
    // vis_.camera_.view[0] = 0;
    // vis_.camera_.view[1] = -1;
    // vis_.camera_.view[2] = 0;
    // vis_.camera_.window_size[0] = 1678;
    // vis_.camera_.window_size[1] = 525;
    // vis_.camera_.window_pos[0] = 2;
    // vis_.camera_.window_pos[1] = 82;
    // vis_.updateCamera();

    draw();
  }
Exemple #19
0
// Constructor
Propulsion::Propulsion(ModelPlane * parent, int ID)
{
	parentObj = parent;
	id = ID;
	XmlRpc::XmlRpcValue list;
	int i;
	char paramMsg[50];
	sprintf(paramMsg, "motor%i/CGOffset", id);
	if(!ros::param::getCached(paramMsg, list)) {ROS_FATAL("Invalid parameters for -%s- in param server!", paramMsg); ros::shutdown();}
	for (i = 0; i < list.size(); ++i) {
		ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
	}
	CGOffset.x = list[0];
	CGOffset.y = list[1];
	CGOffset.z = list[2];

	sprintf(paramMsg, "motor%i/mountOrientation", id);
	if(!ros::param::getCached(paramMsg, list)) {ROS_FATAL("Invalid parameters for -%s- in param server!", paramMsg); ros::shutdown();}
	for (i = 0; i < list.size(); ++i) {
		ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
	}
	// !!! Order mixed because tf::Quaternion::setEuler seems to work with PRY, instead of YPR
	mountOrientation.y = list[0];
	mountOrientation.z = list[1];
	mountOrientation.x = list[2];

	theta = 0; // Initialize propeller angle

	sprintf(paramMsg, "motor%i/chanMotor", id);
	if(!ros::param::getCached(paramMsg, chanMotor)) {ROS_INFO("No MOTOR%i channel selected", id); chanMotor=-1;}
	sprintf(paramMsg, "motor%i/chanGimbal", id);
	if(!ros::param::getCached(paramMsg, chanGimbal)) {ROS_INFO("No GIMBAL%i channel selected", id); chanGimbal=-1;}
	sprintf(paramMsg, "motor%i/gimbalAngle_max", id);
	if(!ros::param::getCached(paramMsg, gimbalAngle_max)) {ROS_INFO("No GIMBALANGLE_MAX%i value selected", id); gimbalAngle_max=0.0;}

	inputMotor = 0.0;
	inputGimbal = 0.0;

	sprintf(paramMsg, "motor%i/rotationDir", id);
	if(!ros::param::getCached(paramMsg, rotationDir)) {ROS_INFO("No ROTATION_DIR%i value selected", id); rotationDir=1.0;}

}
FkLookup::ChainFK::ChainFK(const KDL::Tree& tree, const std::string& root, const std::string& tip):root_name(root),tip_name(tip){
    KDL::Chain chain;
    tree.getChain(root_name, tip_name, chain);
    solver =  new KDL::ChainFkSolverPos_recursive(chain);
    unsigned int  num = chain.getNrOfSegments();
    for(unsigned int i = 0; i < num; ++i){
	const KDL::Joint &joint = chain.getSegment(i).getJoint();
	if (joint.getType() != KDL::Joint::None) joints.insert(std::make_pair(joint.getName(),joints.size()));
    }
    ROS_ASSERT(joints.size() == chain.getNrOfJoints());
}
void RobotBase2DPoseDisplay::targetFrameChanged()
{
  ROS_ASSERT( messages_.size() == arrows_.size() );
  V_RobotBase2DOdom::iterator msg_it = messages_.begin();
  V_Arrow::iterator arrow_it = arrows_.begin();
  V_RobotBase2DOdom::iterator msg_end = messages_.end();
  for ( ; msg_it != msg_end; ++msg_it, ++arrow_it )
  {
    transformArrow( *msg_it, *arrow_it );
  }
}
void IntraProcessSubscriberLink::enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy)
{
  boost::recursive_mutex::scoped_lock lock(drop_mutex_);
  if (dropped_)
  {
    return;
  }

  ROS_ASSERT(subscriber_);
  subscriber_->handleMessage(m, ser, nocopy);
}
Exemple #23
0
//------------------------------------------------------------------------------
SkeletonDBStream::SkeletonDBStream() :
  frame_id_("/world"),
  update_time_(0.01)  // sec
{
  db_pub_ = nh_.advertise<tms_msg_db::TmsdbStamped>("tms_db_data", 1);
  skeleton_sub_ = nh_.subscribe("integrated_skeleton_stream",1,
                &SkeletonDBStream::callback, this);

  ROS_ASSERT("Start sending skeleton data to DB.");
  return;
}
bool DynamicMovementPrimitiveControllerClient::halt(bool wait_for_success)
{
  ROS_ASSERT(initialized_);
  ControllerMapIterator mi = icra2009_controller_clients_.find(current_controller_);
  if(mi == icra2009_controller_clients_.end())
  {
    ROS_ERROR("There is not DMP controller with name >%s<.", current_controller_.c_str());
    return false;
  }
  return mi->second->halt(wait_for_success);
}
    bool ActionExecutorPickupObject::fillGoal(tidyup_msgs::GraspObjectGoal & goal,
            const DurativeAction & a, const SymbolicState & current)
    {
        ROS_ASSERT(a.parameters.size() == 4);
        string location = a.parameters[0];
        goal.pickup_object = a.parameters[1];
        goal.static_object = a.parameters[2];
        goal.arm = a.parameters[3];

        return true;
    }
Exemple #26
0
void Display::setPropertyManager( PropertyManager* manager, const CategoryPropertyWPtr& parent )
{
  ROS_ASSERT(!property_manager_);

  property_manager_ = manager;

  parent_category_ = parent;
  status_property_ = property_manager_->createStatus("Status", property_prefix_, parent_category_, this);

  createProperties();
}
	moveit::planning_interface::MoveItErrorCode ControlTorso::moveTorsoJointPosition(double position)
	{
		std::vector<std::string> torso_joints = torso_group_->getJoints();
		ROS_ASSERT(torso_joints.size() > 0);
		if (!torso_group_->setJointValueTarget(torso_joints[0], position))
		{
			ROS_ERROR("Position out of bounds - not moving torso");
			return moveit::planning_interface::MoveItErrorCode::FAILURE;
		}
		return torso_group_->move();
	}
// copied from channelcontroller
void CalibrateAction::updateVoronoi()
{
    std::vector<IntPoint> obstacles;
    cost_map->getCostmapCopy(costmap_);

    ROS_ASSERT(costmap_.getSizeInCellsX() == voronoi_.getSizeX());
    ROS_ASSERT(costmap_.getSizeInCellsY() == voronoi_.getSizeY());


    for(unsigned int x = 0; x < costmap_.getSizeInCellsX(); x++) {
        for(unsigned int y = 0; y < costmap_.getSizeInCellsY(); y++) {
            if(costmap_.getCost(x, y) >= costmap_2d::LETHAL_OBSTACLE) { // lethal and unknown
                obstacles.push_back(IntPoint(x, y));
            }
        }
    }
    voronoi_.exchangeObstacles(obstacles);
    voronoi_.update(true);
    visualizeVoronoi();
}
SolverInfoProcessor::SolverInfoProcessor(const urdf::Model &robot_model,
	const std::string &tip_name,
	const std::string &root_name)
{
	// setup the IK solver information which contains joint names, joint limits and so on
	boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
	while (link) {
		// check if we have already reached the final link
		if (link->name == root_name) break;

		// if we have reached the last joint the root frame is not in the chain
		// then we cannot build the chain
		if (!link->parent_joint) {
			ROS_ERROR("The provided root link is not in the chain");
			ROS_ASSERT(false);
		}

		// process the joint
		boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(link->parent_joint->name);
		if (!joint) {
			ROS_ERROR("Could not find joint: %s", link->parent_joint->name.c_str());
			ROS_ASSERT(false);
		}

		// add the joint to the chain
		if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
			ROS_DEBUG("Joint axis: %f, %f, %f", joint->axis.x, joint->axis.y, joint->axis.z);

			addJointToChainInfo(link->parent_joint, _solver_info);
		}

		link = robot_model.getLink(link->getParent()->name);
        }

	_solver_info.link_names.push_back(tip_name);

	// We expect order from root to tip, so reverse the order
	std::reverse(_solver_info.limits.begin(), _solver_info.limits.end());
	std::reverse(_solver_info.joint_names.begin(), _solver_info.joint_names.end());
	std::reverse(_solver_info.link_names.begin(), _solver_info.link_names.end());
}
Exemple #30
0
double Jacobian::getManipulabilityMeasure(const Eigen::VectorXd& joint_values)
{
  ROS_ASSERT(initialized_);

  Eigen::MatrixXd jac;

  getJacobian(joint_values, jac);

  Eigen::MatrixXd JJT = jac * jac.transpose();
  return sqrt(JJT.determinant());

}