<commit_before>#include <controller_manager/controller_manager.h>
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/console.h>
#include "robik_hw_ifce.h"
#include "robik_util.h"
#include "robik_api.h"

//arm
#include "robik/ArmControl.h"
robik::ArmControl arm_control_msg;   //message to be send to arduino, reusing memory
ros::Publisher pub_arm_control;
void init_arm_control_message() {
	arm_control_msg.header.frame_id = "base_link";
	arm_control_msg.header.stamp = ros::Time::now();
	arm_control_msg.arm_yaw = 0;
	arm_control_msg.arm_shoulder = 0;
	arm_control_msg.arm_elbow = 0;
	arm_control_msg.arm_roll = 0;
	arm_control_msg.arm_clamp = 0;
}

void statusCallback(const robik::GenericStatus& msg) {
	//arm
	RobikControllers& p_robik_controllers = RobikControllers::get_instance();
	p_robik_controllers.read_from_hw(msg);
}


void RobikControllers::init() { 

   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_yaw("yaw_joint", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_yaw);
   hardware_interface::JointStateHandle state_handle_shoulder("shoulder_joint", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_shoulder);
   hardware_interface::JointStateHandle state_handle_elbow("elbow_joint", &pos[2], &vel[2], &eff[2]);
   jnt_state_interface.registerHandle(state_handle_elbow);
   hardware_interface::JointStateHandle state_handle_roll("roll_joint", &pos[3], &vel[3], &eff[3]);
   jnt_state_interface.registerHandle(state_handle_roll);
   hardware_interface::JointStateHandle state_handle_clamp("clamp_joint", &pos[4], &vel[4], &eff[4]);
   jnt_state_interface.registerHandle(state_handle_clamp);
   
   hardware_interface::JointStateHandle state_handle_left_wheel("wheel_left_joint",&pos[5],&vel[5],&eff[5]);
   jnt_state_interface.registerHandle(state_handle_left_wheel);
   hardware_interface::JointStateHandle state_handle_right_wheel("wheel_right_joint",&pos[6],&vel[6],&eff[6]);
   jnt_state_interface.registerHandle(state_handle_right_wheel);
   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_yaw(jnt_state_interface.getHandle("yaw_joint"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_yaw);
   ROS_INFO("INI arm_control %p %f", &cmd[1],cmd[1]);
   hardware_interface::JointHandle pos_handle_shoulder(jnt_state_interface.getHandle("shoulder_joint"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_shoulder);
   hardware_interface::JointHandle pos_handle_elbow(jnt_state_interface.getHandle("elbow_joint"), &cmd[2]);
   jnt_pos_interface.registerHandle(pos_handle_elbow);
   hardware_interface::JointHandle pos_handle_roll(jnt_state_interface.getHandle("roll_joint"), &cmd[3]);
   jnt_pos_interface.registerHandle(pos_handle_roll);
   hardware_interface::JointHandle pos_handle_clamp(jnt_state_interface.getHandle("clamp_joint"), &cmd[4]);
   jnt_pos_interface.registerHandle(pos_handle_clamp);
   registerInterface(&jnt_pos_interface);

   hardware_interface::JointHandle vel_handle_left_wheel(jnt_state_interface.getHandle("wheel_left_joint"), &cmd[5]);
   jnt_vel_interface.registerHandle(vel_handle_left_wheel);
   hardware_interface::JointHandle vel_handle_right_wheel(jnt_state_interface.getHandle("wheel_right_joint"), &cmd[6]);
   jnt_vel_interface.registerHandle(vel_handle_right_wheel);
   registerInterface(&jnt_vel_interface);

	//IMU
	imu_data.name = "IMU";
    imu_data.frame_id = "base_frame";
    imu_data.orientation = imu_orientation;
    imu_data.angular_velocity = imu_angular_velocity;
    imu_data.linear_acceleration = imu_linear_acceleration;
	hardware_interface::ImuSensorHandle imu_sensor_handle(imu_data);
	imu_interface.registerHandle(imu_sensor_handle);
	registerInterface(&imu_interface);
}

void RobikControllers::read_from_hw (const robik::GenericStatus& msg) {
	pos[0] = map_check_inf(msg.arm_yaw, ARM_RES_MIN_YAW, ARM_RES_MAX_YAW, ARM_DEG_MIN_YAW, ARM_DEG_MAX_YAW) * DEG_TO_RAD;
	pos[1] = map_check_inf(msg.arm_shoulder, ARM_RES_MIN_SHOULDER, ARM_RES_MAX_SHOULDER, ARM_DEG_MIN_SHOULDER, ARM_DEG_MAX_SHOULDER) * DEG_TO_RAD;
	pos[2] = map_check_inf(msg.arm_elbow, ARM_RES_MIN_ELBOW, ARM_RES_MAX_ELBOW, ARM_DEG_MIN_ELBOW, ARM_DEG_MAX_ELBOW) * DEG_TO_RAD;
	pos[3] = map_check_inf(msg.arm_roll, ARM_RES_MIN_ROLL, ARM_RES_MAX_ROLL, ARM_DEG_MIN_ROLL, ARM_DEG_MAX_ROLL) * DEG_TO_RAD;
	pos[4] = map_check_inf(msg.arm_clamp, ARM_RES_MIN_CLAMP, ARM_RES_MAX_CLAMP, ARM_DEG_MIN_CLAMP, ARM_DEG_MAX_CLAMP) * DEG_TO_RAD;
//	ROS_INFO("sub arm_control %f %f %f %f %f", pos[0],pos[1],pos[2],pos[3],pos[4]);
}

void RobikControllers::write_to_hw(){
	//arm
	arm_control_msg.header.stamp = ros::Time::now();
	arm_control_msg.arm_yaw = map_unchecked(cmd[0], ARM_DEG_MIN_YAW, ARM_DEG_MAX_YAW, ARM_MIN_YAW, ARM_MAX_YAW);
	arm_control_msg.arm_shoulder = map_unchecked(cmd[1], ARM_DEG_MIN_SHOULDER, ARM_DEG_MAX_SHOULDER, ARM_MIN_SHOULDER, ARM_MAX_SHOULDER);
	arm_control_msg.arm_elbow = map_unchecked(cmd[2], ARM_DEG_MIN_ELBOW, ARM_DEG_MAX_ELBOW, ARM_MIN_ELBOW, ARM_MAX_ELBOW);
	arm_control_msg.arm_roll = map_unchecked(cmd[3], ARM_DEG_MIN_ROLL, ARM_DEG_MAX_ROLL, ARM_MIN_ROLL, ARM_MAX_ROLL);
	arm_control_msg.arm_clamp = map_unchecked(cmd[4], ARM_DEG_MIN_CLAMP, ARM_DEG_MAX_CLAMP, ARM_MIN_CLAMP, ARM_MAX_CLAMP);
	arm_control_msg.time_to_complete = 50; //20Hz in milliseconds, this is frequency of /robik_arm_control
	ROS_INFO("WRI arm_control %p %f", &cmd[1],cmd[1]);

//	ROS_INFO("pub arm_control %f %f %f %f %f", cmd[0],cmd[1],cmd[2],cmd[3],cmd[4]);
//	ROS_INFO("pub arm_control %u %u %u %u", arm_control_msg.arm_yaw, arm_control_msg.arm_shoulder, arm_control_msg.arm_elbow, arm_control_msg.arm_roll);
	pub_arm_control.publish(arm_control_msg);
}

ros::Time RobikControllers::get_time(){
  return ros::Time::now();
}

ros::Duration RobikControllers::get_period(){
  ros::Time current_time = ros::Time::now();
  ros::Duration period = current_time - last_time;
  last_time = current_time;
  return period;
}

int main(int argc, char **argv) {
<commit_msg>casting<commit_after>#include <controller_manager/controller_manager.h>
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/console.h>
#include "robik_hw_ifce.h"
#include "robik_util.h"
#include "robik_api.h"

//arm
#include "robik/ArmControl.h"
robik::ArmControl arm_control_msg;   //message to be send to arduino, reusing memory
ros::Publisher pub_arm_control;
void init_arm_control_message() {
	arm_control_msg.header.frame_id = "base_link";
	arm_control_msg.header.stamp = ros::Time::now();
	arm_control_msg.arm_yaw = 0;
	arm_control_msg.arm_shoulder = 0;
	arm_control_msg.arm_elbow = 0;
	arm_control_msg.arm_roll = 0;
	arm_control_msg.arm_clamp = 0;
}

void statusCallback(const robik::GenericStatus& msg) {
	//arm
	RobikControllers& p_robik_controllers = RobikControllers::get_instance();
	p_robik_controllers.read_from_hw(msg);
}


void RobikControllers::init() { 

   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_yaw("yaw_joint", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_yaw);
   hardware_interface::JointStateHandle state_handle_shoulder("shoulder_joint", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_shoulder);
   hardware_interface::JointStateHandle state_handle_elbow("elbow_joint", &pos[2], &vel[2], &eff[2]);
   jnt_state_interface.registerHandle(state_handle_elbow);
   hardware_interface::JointStateHandle state_handle_roll("roll_joint", &pos[3], &vel[3], &eff[3]);
   jnt_state_interface.registerHandle(state_handle_roll);
   hardware_interface::JointStateHandle state_handle_clamp("clamp_joint", &pos[4], &vel[4], &eff[4]);
   jnt_state_interface.registerHandle(state_handle_clamp);
   
   hardware_interface::JointStateHandle state_handle_left_wheel("wheel_left_joint",&pos[5],&vel[5],&eff[5]);
   jnt_state_interface.registerHandle(state_handle_left_wheel);
   hardware_interface::JointStateHandle state_handle_right_wheel("wheel_right_joint",&pos[6],&vel[6],&eff[6]);
   jnt_state_interface.registerHandle(state_handle_right_wheel);
   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_yaw(jnt_state_interface.getHandle("yaw_joint"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_yaw);
   ROS_INFO("INI arm_control %p %f", &cmd[1],cmd[1]);
   hardware_interface::JointHandle pos_handle_shoulder(jnt_state_interface.getHandle("shoulder_joint"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_shoulder);
   hardware_interface::JointHandle pos_handle_elbow(jnt_state_interface.getHandle("elbow_joint"), &cmd[2]);
   jnt_pos_interface.registerHandle(pos_handle_elbow);
   hardware_interface::JointHandle pos_handle_roll(jnt_state_interface.getHandle("roll_joint"), &cmd[3]);
   jnt_pos_interface.registerHandle(pos_handle_roll);
   hardware_interface::JointHandle pos_handle_clamp(jnt_state_interface.getHandle("clamp_joint"), &cmd[4]);
   jnt_pos_interface.registerHandle(pos_handle_clamp);
   registerInterface(&jnt_pos_interface);

   hardware_interface::JointHandle vel_handle_left_wheel(jnt_state_interface.getHandle("wheel_left_joint"), &cmd[5]);
   jnt_vel_interface.registerHandle(vel_handle_left_wheel);
   hardware_interface::JointHandle vel_handle_right_wheel(jnt_state_interface.getHandle("wheel_right_joint"), &cmd[6]);
   jnt_vel_interface.registerHandle(vel_handle_right_wheel);
   registerInterface(&jnt_vel_interface);

	//IMU
	imu_data.name = "IMU";
    imu_data.frame_id = "base_frame";
    imu_data.orientation = imu_orientation;
    imu_data.angular_velocity = imu_angular_velocity;
    imu_data.linear_acceleration = imu_linear_acceleration;
	hardware_interface::ImuSensorHandle imu_sensor_handle(imu_data);
	imu_interface.registerHandle(imu_sensor_handle);
	registerInterface(&imu_interface);
}

void RobikControllers::read_from_hw (const robik::GenericStatus& msg) {
	pos[0] = map_check_inf(msg.arm_yaw, ARM_RES_MIN_YAW, ARM_RES_MAX_YAW, ARM_DEG_MIN_YAW, ARM_DEG_MAX_YAW) * DEG_TO_RAD;
	pos[1] = map_check_inf(msg.arm_shoulder, ARM_RES_MIN_SHOULDER, ARM_RES_MAX_SHOULDER, ARM_DEG_MIN_SHOULDER, ARM_DEG_MAX_SHOULDER) * DEG_TO_RAD;
	pos[2] = map_check_inf(msg.arm_elbow, ARM_RES_MIN_ELBOW, ARM_RES_MAX_ELBOW, ARM_DEG_MIN_ELBOW, ARM_DEG_MAX_ELBOW) * DEG_TO_RAD;
	pos[3] = map_check_inf(msg.arm_roll, ARM_RES_MIN_ROLL, ARM_RES_MAX_ROLL, ARM_DEG_MIN_ROLL, ARM_DEG_MAX_ROLL) * DEG_TO_RAD;
	pos[4] = map_check_inf(msg.arm_clamp, ARM_RES_MIN_CLAMP, ARM_RES_MAX_CLAMP, ARM_DEG_MIN_CLAMP, ARM_DEG_MAX_CLAMP) * DEG_TO_RAD;
//	ROS_INFO("sub arm_control %f %f %f %f %f", pos[0],pos[1],pos[2],pos[3],pos[4]);
}

void RobikControllers::write_to_hw(){
	//arm
	arm_control_msg.header.stamp = ros::Time::now();
	arm_control_msg.arm_yaw = (unsigned int) map_unchecked(cmd[0], ARM_DEG_MIN_YAW, ARM_DEG_MAX_YAW, ARM_MIN_YAW, ARM_MAX_YAW);
	arm_control_msg.arm_shoulder = (unsigned int) map_unchecked(cmd[1], ARM_DEG_MIN_SHOULDER, ARM_DEG_MAX_SHOULDER, ARM_MIN_SHOULDER, ARM_MAX_SHOULDER);
	arm_control_msg.arm_elbow = (unsigned int) map_unchecked(cmd[2], ARM_DEG_MIN_ELBOW, ARM_DEG_MAX_ELBOW, ARM_MIN_ELBOW, ARM_MAX_ELBOW);
	arm_control_msg.arm_roll = (unsigned int) map_unchecked(cmd[3], ARM_DEG_MIN_ROLL, ARM_DEG_MAX_ROLL, ARM_MIN_ROLL, ARM_MAX_ROLL);
	arm_control_msg.arm_clamp = (unsigned int) map_unchecked(cmd[4], ARM_DEG_MIN_CLAMP, ARM_DEG_MAX_CLAMP, ARM_MIN_CLAMP, ARM_MAX_CLAMP);
	arm_control_msg.time_to_complete = 50; //20Hz in milliseconds, this is frequency of /robik_arm_control
	ROS_INFO("WRI arm_control %p %f", &cmd[1],cmd[1]);

//	ROS_INFO("pub arm_control %f %f %f %f %f", cmd[0],cmd[1],cmd[2],cmd[3],cmd[4]);
//	ROS_INFO("pub arm_control %u %u %u %u", arm_control_msg.arm_yaw, arm_control_msg.arm_shoulder, arm_control_msg.arm_elbow, arm_control_msg.arm_roll);
	pub_arm_control.publish(arm_control_msg);
}

ros::Time RobikControllers::get_time(){
  return ros::Time::now();
}

ros::Duration RobikControllers::get_period(){
  ros::Time current_time = ros::Time::now();
  ros::Duration period = current_time - last_time;
  last_time = current_time;
  return period;
}

int main(int argc, char **argv) {

{'fraction_non_alphanumeric': 0.12243574150652721, 'fraction_numerical': 0.00989215924754723, 'mean_word_length': 8.89093825180433, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4854186', 'n_tokens_mistral': 4832, 'n_tokens_neox': 4754, 'n_words': 602}