void BodyKinematics::teleopBodyCmd(const crab_msgs::BodyCommandConstPtr &body_cmd){
if (body_cmd->cmd == body_cmd->STAND_UP_CMD){
ROS_ERROR("STAND_UP_CMD");
ros::Rate r(25);
while (bs.z >= -z){
bs.z -= 0.0025;
r.sleep();
if (calculateKinematics(&bs)){
joints_pub.publish(legs);
}
}
}
if (body_cmd->cmd == body_cmd->SEAT_DOWN_CMD){
ROS_ERROR("SEAT_DOWN_CMD");
ros::Rate r(25);
while (bs.z <= -0.016){
bs.z += 0.0025;
r.sleep();
if (calculateKinematics(&bs)){
joints_pub.publish(legs);
}
}
}
// if (body_cmd->cmd == body_cmd->IMU_START_CMD){
// ROS_ERROR("IMU_START_CMD");
// ros::Rate r(25);
// while (bs.z >= -0.08){
// bs.z -= 0.0025;
// r.sleep();
// if (calculateKinematics(&bs)){
// joints_pub.publish(legs);
// }
// }
// }
}
bool BodyKinematics::init() {
std::string robot_desc_string;
// Get URDF XML
if (!node.getParam("robot_description", robot_desc_string)) {
ROS_FATAL("Could not load the xml from parameter: robot_description");
return false;
}
// Get Root and Tip From Parameter Server
node.param("root_name_body", root_name, std::string("leg_center"));
node.param("tip_name_body", tip_name, std::string("coxa"));
node.param("clearance", z, 0.045);
// Load and Read Models
if (!loadModel(robot_desc_string)) {
ROS_FATAL("Could not load models!");
return false;
}
client = node.serviceClient<crab_msgs::GetLegIKSolver>("/crab_leg_kinematics/get_ik");
joints_pub = node.advertise<crab_msgs::LegsJointsState>("joints_to_controller", 1);
body_move_sub = node.subscribe<crab_msgs::BodyState>("/teleop/move_body", 1, &BodyKinematics::teleopBodyMove, this);
body_cmd_sub = node.subscribe<crab_msgs::BodyCommand>("/teleop/body_command", 1, &BodyKinematics::teleopBodyCmd, this);
bs.leg_radius = 0.11;
bs.z = -0.016;
ros::Duration(1).sleep();
if (calculateKinematics(&bs)){
joints_pub.publish(legs);
}
ROS_INFO("Ready to receive teleop messages... ");
return true;
}
void BodyKinematics::teleopBodyMove(const crab_msgs::BodyStateConstPtr &body_state){
bs.x = body_state->x;
bs.y = body_state->y;
bs.z = body_state->z;
bs.pitch = body_state->pitch;
bs.roll = body_state->roll;
bs.yaw = body_state->yaw;
bs.leg_radius = body_state->leg_radius;
if (calculateKinematics(&bs)){
joints_pub.publish(legs);
}
}
//void BodyKinematics::teleopBodyMove(const spider_msgs::BodyStateConstPtr &body_state){
void BodyKinematics::teleopBodyMove(const spider_msgs::msg::BodyState::SharedPtr &body_state){
bs.x = body_state->x;
bs.y = body_state->y;
bs.z = body_state->z;
bs.pitch = body_state->pitch;
bs.roll = body_state->roll;
bs.yaw = body_state->yaw;
bs.leg_radius = body_state->leg_radius;
if (calculateKinematics(&bs)){
//joints_pub.publish(legs);
//joints_pub->publish(legs);
}
}
//void BodyKinematics::teleopBodyCmd(const spider_msgs::BodyCommandConstPtr &body_cmd){
void BodyKinematics::teleopBodyCmd(const spider_msgs::msg::BodyCommand::SharedPtr &body_cmd){
if (body_cmd->cmd == body_cmd->STAND_UP_CMD){
// ROS_ERROR("STAND_UP_CMD");
// ros::Rate r(25);
rclcpp::rate::Rate loop_rate(25);
while (bs.z >= -z){
bs.z -= 0.0025;
//r.sleep();
if (calculateKinematics(&bs)){
//joints_pub.publish(legs);
}
}
}
if (body_cmd->cmd == body_cmd->SEAT_DOWN_CMD){
// ROS_ERROR("SEAT_DOWN_CMD");
// ros::Rate r(25);
while (bs.z <= -0.016){
bs.z += 0.0025;
//r.sleep();
if (calculateKinematics(&bs)){
//joints_pub.publish(legs);
}
}
}
if (body_cmd->cmd == body_cmd->IMU_START_CMD){
// ROS_ERROR("IMU_START_CMD");
// ros::Rate r(25);
while (bs.z >= -0.08){
bs.z -= 0.0025;
// r.sleep();
if (calculateKinematics(&bs)){
// joints_pub.publish(legs);
}
}
}
}
bool BodyKinematics::init() {
std::string robot_desc_string;
// Get URDF XML
//if (!node.getParam("robot_description", robot_desc_string)) {
// ROS_FATAL("Could not load the xml from parameter: robot_description");
// return false;
// }
// Get Root and Tip From Parameter Server
//node.param("root_name_body", root_name, std::string("leg_center"));
//node.param("tip_name_body", tip_name, std::string("coxa"));
//node.param("clearance", z, 0.045);
// Load and Read Models
if (!loadModel(robot_desc_string)) {
// ROS_FATAL("Could not load models!");
return false;
}
//client = node.serviceClient<spider_msgs::GetLegIKSolver>("/spider_leg_kinematics/get_ik");
//auto client = node->create_client<spider_msgs::srv::GetLegIKSolver>("/spider_leg_kinematics/get_ik");
//joints_pub = node.advertise<spider_msgs::LegsJointsState>("joints_to_controller", 1);
//auto joints_pub = node->create_publisher<spider_msgs::msg::LegsJointsState>("joints_to_controller", rmw_qos_profile_default);
//body_move_sub = node.subscribe<spider_msgs::BodyState>("/teleop/move_body", 1, &BodyKinematics::teleopBodyMove, this);
//auto body_move_sub = node->create_subscription<spider_msgs::msg::BodyState>("/teleop/move_body", rmw_qos_profile_default, &BodyKinematics::teleopBodyMove, this);
//body_cmd_sub = node.subscribe<spider_msgs::BodyCommand>("/teleop/body_command", 1, &BodyKinematics::teleopBodyCmd, this);
//auto body_cmd_sub = node->create_subscription<spider_msgs::msg::BodyCommand>("/teleop/body_command", rmw_qos_profile_default, &BodyKinematics::teleopBodyCmd, this);
bs.leg_radius = 0.11;
bs.z = -0.016;
//ros::Duration(1).sleep();
if (calculateKinematics(&bs)){
// joints_pub.publish(legs);
//joints_pub->publish(legs);
}
//ROS_INFO("Ready to receive teleop messages... ");
printf("%s\n","Ready to receive teleop messages... ");
return true;
}
