int main(int argc, char **argv)
{
ros::init(argc, argv, "move_joints", ros::init_options::AnonymousName);
ros::NodeHandle nh;
// start a ROS spinning thread
ros::AsyncSpinner spinner(1);
spinner.start();
std::vector<std::string> joint_names;
getJointNames("controller_joint_names",joint_names,nh);
Client_t* client;
// client = new Client_t("joint_trajectory_action",true);
client = new Client_t("follow_joint_trajectory",true);
client->waitForServer(ros::Duration(5.0));
control_msgs::FollowJointTrajectoryGoal goal;
moveGoal(joint_names,&goal);
client->sendGoal(goal);
client->waitForResult(ros::Duration(5.0));
ros::waitForShutdown();
}
bool RobotStateInterface::init(SmplMsgConnection* connection)
{
std::vector<std::string> joint_names;
if (!getJointNames("controller_joint_names", joint_names))
ROS_WARN("Unable to read 'controller_joint_names' param. Using standard 6-DOF joint names.");
return init(connection, joint_names);
}
bool RobotStateInterface::init(SmplMsgConnection* connection)
{
std::vector<std::string> joint_names;
if (!getJointNames("controller_joint_names", "robot_description", joint_names))
{
ROS_ERROR("Failed to initialize joint_names. Aborting");
return false;
}
return init(connection, joint_names);
}
void KDeviceLists::fillJointKeys(std::vector<std::string> &Keys, std::string const & postFix)
{
//std::vector< std::string> Keys;
// Joints list
std::vector<std::string> const& Names = getJointNames();
Keys.resize(Names.size());
for(unsigned i = 0; i < Names.size(); i++)
Keys[i] = preFix + Names[i] + postFix;
//return Keys;
}
domNode* getRootJointNode(const domSkin* skin)
{
std::vector<std::string> names;
getJointNames(skin, names);
daeSIDResolver resolver(const_cast<domSkin*>(skin)->getDocument()->getDomRoot(), names[0].c_str());
daeElement* element = resolver.getElement();
if (element && element->typeID() == COLLADA_TYPE::NODE)
{
domNode* node = daeSafeCast<domNode>(resolver.getElement());
return node;
}
return NULL;
}
void findChannelsTargetingJoints(const domSourceRef& source, std::list<domChannelRef>& channels, std::list<domNodeRef>& nodes)
{
std::vector<std::string> jointNames;
getJointNames(source, jointNames);
for (std::vector<std::string>::iterator i = jointNames.begin(); i != jointNames.end(); ++i)
{
daeSIDResolver resolver(source->getDocument()->getDomRoot(), i->c_str());
daeElement* element = resolver.getElement();
if (element && element->typeID() == COLLADA_TYPE::NODE)
{
domNodeRef node = daeSafeCast<domNode>(element);
nodes.push_back(node);
getAnimationChannels(node, channels);
}
}
}
void getJointNames(const domSkin* skin, std::vector<std::string>& list)
{
const domSkin::domJointsRef& joints = skin->getJoints();
const domInputLocal_Array& inputArray = joints->getInput_array();
size_t inputCount = inputArray.getCount();
for (size_t i = 0; i < inputCount; ++i)
{
const domInputLocalRef input = inputArray.get(i);
const char* semantic = input->getSemantic();
if (strcmp(semantic, "JOINT") == 0)
{
daeElement* sourceElement = input->getSource().getElement();
if (sourceElement)
{
const domSourceRef source = daeSafeCast<domSource>(sourceElement);
getJointNames(source, list);
}
}
}
}
void jointPublish(ros::Publisher jP) {
updateMotor();
/* ************** Publish joint angles ************** */
sensor_msgs::JointState msg;// = boost::make_shared<sensor_msgs::JointState>();
// msg.reset(new sensor_msgs::JointState);
std::vector<std::string> joint_names;
std::vector<float> angles;
std::vector<float> vels;
getJointNames(joint_names);
getJointAngles(angles);
getJointVelocities(vels);
for (int i = 0; i < 2 ; ++i) {
msg.name.push_back(joint_names[i]);
msg.position.push_back(angles[i]);
msg.velocity.push_back(vels[i]);
}
msg.header.stamp = ros::Time::now();
jP.publish(msg);
}
bool RobotStateInterface::init(SmplMsgConnection* connection)
{
if (this->version_0_)
{
std::vector<std::string> joint_names;
if (!getJointNames("controller_joint_names", "robot_description", joint_names))
ROS_WARN("Unable to read 'controller_joint_names' param. Using standard 6-DOF joint names.");
return init(connection, joint_names);
}
else
{
std::map<int, RobotGroup> robot_groups;
std::string value;
ros::param::search("topics_list", value);
XmlRpc::XmlRpcValue topics_list_rpc;
ros::param::get(value, topics_list_rpc);
std::vector<XmlRpc::XmlRpcValue> topics_list;
for (int i = 0; i < topics_list_rpc.size(); i++)
{
XmlRpc::XmlRpcValue state_value;
state_value = topics_list_rpc[i];
topics_list.push_back(state_value);
}
std::vector<XmlRpc::XmlRpcValue> groups_list;
// TODO(thiagodefreitas): check the consistency of the group numbers
for (int i = 0; i < topics_list[0]["state"].size(); i++)
{
XmlRpc::XmlRpcValue group_value;
group_value = topics_list[0]["state"][i];
groups_list.push_back(group_value);
}
for (int i = 0; i < groups_list.size(); i++)
{
RobotGroup rg;
std::vector<std::string> rg_joint_names;
XmlRpc::XmlRpcValue joints;
joints = groups_list[i]["group"][0]["joints"];
for (int jt = 0; jt < joints.size(); jt++)
{
rg_joint_names.push_back(static_cast<std::string>(joints[jt]));
}
XmlRpc::XmlRpcValue group_number;
group_number = groups_list[i]["group"][0]["group_number"];
int group_number_int = static_cast<int>(group_number);
XmlRpc::XmlRpcValue name;
std::string name_string;
name = groups_list[i]["group"][0]["name"];
name_string = static_cast<std::string>(name);
XmlRpc::XmlRpcValue ns;
std::string ns_string;
ns = groups_list[i]["group"][0]["ns"];
ns_string = static_cast<std::string>(ns);
rg.set_group_id(group_number_int);
rg.set_joint_names(rg_joint_names);
rg.set_name(name_string);
rg.set_ns(ns_string);
robot_groups[group_number] = rg;
}
return init(connection, robot_groups);
}
}
