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); }
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; } }
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; }
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(); }
// 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); }
//------------------------------------------------------------------------------ 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; }
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()); }
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()); }